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ABSTRACT 


An autopilot is designed using Sliding Mode Control Theory that can control the 
speed and heading of a full six degree of freedom, nonlinear model of the Navy’s SDV 
Mark 9 vehicle. The control laws are based on a simplified linear model that allows speed 
and heading to be treated as separate systems. Once control of heading and speed is 
established, depth control is then achieved by a third control law. Although they are 
developed separately, the three individual control laws act simultaneously to provide 
robust control of speed, heading, and depth of the nonlinear model of the vehicle. Line 
of Sight Guidance is used to convert the way points provided by the mission planner into 
commands for heading to which the autopilot responds. The performance of the autopilot 
is evaluated over a wide range of speeds to demonstrate its robustness. In addition, the 
effects of current are simulated and the autopilot is modified to compensate for the 
presence of a strong current. 
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I. INTRODUCTION 


A. GENERAL 

There has been an increased interest recently in the need for autonomous underwater 
vehicles (AUV) in both the Navy and private industry. A variety of unclassified missions 
include ASW, decoy, survey, reconnaissance, and ocean engineering work. As the cost 
of manned submarines increases, there are significant advantages to using cheaper 
unmanned vehicles. The AUV should be able to maneuver freely in the ocean 
environment with respect to speed, heading, and depth in order to carry out its missions. 
Such maneuvering requirements have to be easily accomplished by a low level active 
control system, and in the presence of environmental and physical uncertainty. 

All information conceming the environment of a vehicle is detected by the sensing 
instrumentation on board the vehicle and sent to the higher level intelligence systems to 
be interpreted in order to carry out the mission. The dynamics of underwater vehicles are 
described by highly nonlinear systems of equations with uncertain coefficients and 
disturbances that are difficult to measure. Robust control using Sliding Mode Control 
Theory is reputed to provide accurate control of nonlinear systems despite unmodeled 


system dynamics and disturbances making it a likely candidate for designing the control 


laws that will govern the autopilot function of unmanned vehicles. 


B. AIM OF THIS STUDY 

The aim of this thesis is to develop a robust autopilot that will control speed, 
heading, and depth by the application of Sliding Mode Control Theory. The full six 
degree of freedom equations of motion that describe the vehicle are simplified and 
linearized to the point that they can be treated as three separate, independent, single input 
single output systems. The control laws for speed, heading, and depth are developed 
independently and then combined together to attain effective control of a simple linear 
model first and ultimately the complete nonlinear model for the vehicle. 

Once the autopilot is verified, it is coupled with a Line of Sight Guidance scheme 
to allow the autopilot to interface with the higher levels of decision maida that provide 
way points to describe a desired path for the vehicle. The merits of the total system are 
then evaluated for robustness in terms of the range of operating speed over which it can 
successfully control the vehicle when compared to the nominal operating speed about 
which the equations of motion were initially linearized. 

Finally, the effect of current on Line of Sight Guidance is explored and eGnementa 
made resulting in the development of a highly robust and effective system for sonteeliite 
the next generation of autonomous underwater vehicles under construction at the Naval 


Postgraduate School and elsewhere in private industry. 


C. THESIS OUTLINE 


In Chapter 2 the equations of motion for the full six degree of freedom model of 


the AUV are simplified and linearized. A model for the vehicle is then built using the 


Matrix-x software package (Copyright 1989 by Integrated Systems Inc.) and sliding mode 
control laws are developed, assuming full state feedback, to control the speed and heading 
of this simple linear model. 

Chapter 3 shows the application of the control laws developed in Chapter 2 to the 
full, nonlinear, six degree of freedom model that represents the SDV-9 vehicle currently 
in use by the Navy. In addition, the depth control law which was developed previously 
at NPS by Joo-No Sur [Ref. 1] was included to attain control of depth. 

In Chapter 4 an observer for sway velocity is designed and tested. It is ultimately 
determined that sway velocity is better treated as a disturbance and consequently it is set 
to zero wherever it appears in the control law equations. 

Chapter 5 explores using Line of Sight Guidance to allow the way points provided 
by the planning function of the AUV to be interpreted into commands for heading. 

Chapter 6 investigates the speed envelope of 3 ft/sec to 50 ft/sec over which the 

“ control laws os valid even though the initial linearization was done around an operating 
speed of 6 ft/sec. This shows the robustness of the control laws. 

Chapter 7 shows the effect of current on Line of Sight Guidance and develops - 


modified control law, including current compensation, that improves the autopilot’s 


effectiveness in currents with speeds up to 50% of the operating speed. 


II. LINEAR MODELING WITH MATRIX-X 


A. INTRODUCTION 
Because the vehicle dynamic equations of motion are relatively easy to linearize, 
the first step of control design for the autopilot was to consider the AUV to be | 
| represented by a nominal linear model. In linearizing the equations of motion it was found 
that the equations became uncoupled to the extent that speed control and heading control 
could be addressed separately. This led to considering the control of heading and speed 
as two iidependent SISO systems. Using this design approach, the two inputs provided 
to the autopilot are the desired heading and the desired speed. No attempt is made to 
control depth at this stage in the autopilot design. In fact, it is assumed that all motion 
is confined to the horizontal plane. The block diagram representing the autopilot is 
shown in Figure 1. This is the actual Matrix-x system build diagram in superblock format. 
The remaining portion of this chapter will be devoted to explaining each of these blocks 


individually as the autopilot control design, based on the nominal model, is described. 


B. AUV DYNAMIC MODEL 


1. Identification of symbols 
All the symbols that will be used to develop the linear model for the AUV are | 


identified in the execution program shown in Appendix A. They are listed along with 


their units and in the case of constant parameters, numerical values are given. 


SMC Heading Control 


AUY Dynamic Model 


Sway velocity 


Surge velocit TD 


SHC Speed Control 


Figure 1. Autopilot Block Diagram 


2. Simplification and linearization 

The nonlinear equations of motion for this vehicle were taken from work done 
previously at NPS by Boncal [Ref. 2] who used the dynamic model as established by 
Crane, Summey, et al [Ref. 3], as representative of the SDV Mark 9 vehicle. While this | 
vehicle is not identical to any specifically planned AUV, it remains a useful vehicle for 
the study of dynamics and control issues. 

Only the equations for motion in the horizontal plane are developed in this 
chapter. In addition, the complete form of these equations will not be shown. Instead, the 
assumptions used to simplify them will be stated and the resulting simplified equations 
will be shown in their linearized form. It should be noted that the linearization was done 
around a constant speed 0 = 6.0 ft/sec and a coueaneaiing constant motor rpm fi = 500 


rpm, with the vehicle in level flight. 


a. Assumptions 
¢ Motion is confined to the horizontal plane: 
W, Ps a, 8, Ps d 8,» 8, = 0) 
¢ Some dynamic coefficients are small and can be neglected: 


(Y, Vy Vg Doe = 0) 


¢ The AUV is neutrally buoyant. 


+ By design X,,Y,,Z, are negligible. 


* Cross flow drag terms tend to cancel each other as far as steering moments are 
concerned. 


b. Longitudinal equation of motion 


w= -2uur+pnd, (2.1) 


Where: 
a =(2p L?Cy)/ (2 mass - p LX, ) 
Cyo = -00385 + 1.296E-17 [u Likvis - 1.2E-7 Pr 
B = (012)? « 
c. Lateral equation of motion 
: | NM PSY, C9e Le GPa G Se (2.2) 
Where: 


m, = (mass -p L? Y,)/2 
m,=(pL*Y,)/2 


Y= (pl? ¥,@)/2 


-Yew(p Ll yu)]2 


¥,=(pL’? ¥, uw*)/2 
d. Yaw equation of motion 
mv +m = N,v +N, 1r +N, 6, (2.3) 


Where: 
m, = (-p L‘N,)/ 2 
m, =1,-(p L°N,)/2 
N,=(p L°N, uw) /2 
N, = (p Lin, #)/2 


N,=(p L°N, uw?) /2 


e. Euler angle rate 


ber (2.4) 
Sf Inertial position rates 
¥ = u cos(p) - v sin(y) (2.5) 
y =u sin() + v cos(y) | (2.6) 
8 


3. State space configuration 


a. SISOI 
The state space equation for system 1 that will be used to attain speed 
control is simply the longitudinal equation of motion previously developed and considered 


to be in the standard state space form shown below. 


X, = (A) X, + (BJ YU, 


The components of these matrices are given by equation (2.7). 
é=[-auju+ [Pn] d, (2.7) 


b. SISO2 
To find the state space equation for heading control, equations (2.2) and 


(2.3) must be treated as coupled. This was done as follows: 


Represent equations (2.2) and (2.3) as: 


r 


an(?)-c6i[ 


Consequently, this equation can be solved as follows: 


(stun af f =[D] “| a 
or br 


| Combining equation (2.4) with equation (2.8) results in the state space form 
to be used for heading control. Since two systems are being dealt with at the same time 


this system will be represented by the following state space equation. 


x, = [A,] X, + [B,] U, 


Where these matrices are defined by equation (2.9). 


y v 
v| =O diy daly] + [4s] 8 (2.9) 
‘ 0 d,, dy) : ts 


Equations (2.7) and (2.9) are represented graphically in the Matrix-x system 


build diagram shown in Figure 2. Again notice that they represent two independent 
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systems which leads to the next step of determining the control laws for heading control 
and speed control by considering them as separate SISO systems. The triangles represent 


gain values calculated in the execution program shown in Appendix A. 


C. SISO1 SLIDING MODE SPEED CONTROL 

At this point the control of speed is easily achieved using sliding mode control 
theory. The basis of this theory and its application to controlling the dive maneuvers of 
AUV’s is documented by Joo-No Sur [Ref. 1]. Consequently, the theory of sliding mode 
control will not be included here but rather its application will be shown. Further details 
are given in the tutorial by Decarlo et al. [Ref. 4], and in the paper by Yoeger and Slotine 
[Ref. 5] for application to underwater vehicles. 

Recalling from equation (27) that the state vector for this system is simply the 
surge velocity (u) and the input is the motor rpm (5,), the equation for the sliding surface 
is written as a weighted sum of the error between the state variables and their desired 


values. 


o, = 5,(u - uy) — (2.10) 


According to Liapunov Stability [Ref. 6], global asymptotic stability of the speed error 


is guaranteed by the condition: _ 


6,() 0, < 0 


11 


Figure 2. Linear AUV Dynamic Model 
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For SISO1 this results in the following equation. 


6,() = -(nj) sen(o,)) (2.11) 


The symbol n, represents the sliding coefficient for the speed control law. To eliminate 
chattering, the sgn function can be replaced without loss of generality by the Matrix-x 
function satsgn. Satsgn still acts like a switch, but has the advantage of allowing a linear 
increase to the prescribed saturation value which in this ass is unity. As a result of this 
substitution, and considering the time dependence of 6, to be understood,the expression 


now has the form: 


6, = -(q}) satsgn(o,) (2.12) 


The speed control law can be obtained by setting the derivative of Equation (2.10) equal 
to Equation (2.12) and substituting for u from the state space equation for SISO1. The 


resulting expression for the motor input is: 


&, = 1 -ni satsgn(c,) - 5s, auu]/] (s, Bn) (2.13) 
At this point the values of n, and s, must be determined. Since there is only one 
state variable to consider, s, = 1. The choice of 1), will be made through trial and error - 


using Matrix-x simulation. In general, increasing n, will make the system more responsive 


while reducing it will make the system more sluggish. 
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The speed control law is represented in Matrix-x system build in Figure 3. Notice 
that the control signal (5,) is made up of two parts, one representing the linear feedback 


term and the other the nonlinear switching term as shown as follows: 


8,=6,+5 (2.14) 


Notice too, that an upper limit of 500 rpm has been included in the system build to 


simulate the maximum allowed motor rpm. 


St Satsgn Hatat . RPM Limit 


Saturatio: 


Figure 3. SISO1 Sliding Mode Speed Control Block Diagram 


D. SISO2 SLIDING MODE HEADING CONTROL 
Sliding mode control theory is used to achieve heading control by using the state 


space equation for SISO2. First the equation for the sliding surface is written as a 


14 


weighted sum of the error between the state variables and their desired values where the 


weights are the coefficients of the sliding surface [Ref. 4]. 


0, = 5(W - Wy) + 5,(¥ - vg) + 7 - 7g) (2.15) 


This equation is simplified by noting that the desired values of sway velocity and angular 
velocity are zero. By making this substitution the sliding surface equation for heading 


control becomes: 


0, = SW - ¥y) + 5,0) + 5,0) (2.16) 


Again using Liapunov Stability Theory to meet the requirements for global asymptotic 


stability the following expression is obtained for SISO2. 


6, = -(n,)” satsgn(o,) (2.17) 


Next, 6, is found by taking the derivative of equation (2.16), realizing that wy, at this 


stage, is considered to be a constant. 


6, = sh) + 5,0) + 5,09) (2.18) 


By equating equation (2.17) and (2.18) and substituting from the SISO2 state space 
equation for the derivatives of the state variables the heading control law is obtained as 


shown. 
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UL a cm Sl ol as ll (2.19) 


, (8,4,, + 5,4,3) 


Notice again, that this control law can again be considered to have two distinct parts as 


shown below. 


The linear feedback part of the control law is tinea to give the system the 
desired dynamics while on the sliding surface. The nonlinear term is responsible for 
getting the system to the sliding surface initially and compensating for any unmodeled 
behavior that would drive the vehicle off of the sliding surface once it got there. Once on 
the sliding surface, there is no need for the nonlinear term unless there is uncertainty in 


the model [Ref. 4]. The equations for both of these terms are as follows: 
§ = -[NN, r + NN, v] /DD, (2.20) 
8, = (-(n,)/DD,) satsgn(o,) (2.21) 
Where: 


NN, = S, + 83 d,, + 54 d,, 


NN, = 53 di. + 54 dy, 


DD, = s; d,, + 54 dy 


Unlike the speed control subsystem, the sliding surface for heading control is 
multidimensional. In this case the value for S™=[s,,8,,8,]" is determined by considering 
the closed loop dynamics of this system in the sliding condition (6,0 = 0). Under this 


condition the state space equation for SISO2 becomes: 


X, = [A,] X, + [B) 8, (2.22) 


By writing equation (2.20) in terms of S* and the state space matrices, the following 


relationship is attained. 


§ = -(S7 B71 S™A, X, (2.23) 


Combining equations (2.22) and (2.23) gives: 


X, = 1A, - BS? B'S” A) X, (2.24) 


Note that equation (2.24) is in the form: 


X, = IA, - B, KI X, 


Where: 
K = (S7 B,)' S7 A, 


Consequently, equation (2.24) can be represented as: 
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X, = [AC,] X, 


In this form [AC,] is the closed loop dynamics matrix. At this point K can be found 
using pole placement techniques. This is done by specifying the desired poles that will 
determine the behavior on the sliding plane. It should be noted here that in accordance 
with Sliding Mode Theory, one of these poles must be at the origin thus leaving 2 ease 
to determine the closed loop bandwidth and damping [Ref. 4]. Using Matrix-x this is done 


as follows: _ 


K = POLEPLACE(AC,,B,,POLES,) 


Now that a value for K can be found, S" is obtained by multiplying both sides of 


the equation for K by S"B, . This gives the result: 


ST [AC,] = 0 


Which is also correctly expressed as equation (2.25). 


[ AC, ]’S = 0 (2.25) 


At this point the fact that [AC,]" has one eigenvalue that is zero is utilized to 
complete the solution of S by considering the standard eigenvalue problem ((AC,J' -DV 


= @. For the case when the eigenvalue is zero, [AC,]'V = 0 . Comparing this expression 
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with Soantion (2.25) it becomes apparent that the eigenvector V in the general case 
becomes the vector S for our circumstances [Ref. 7]. Consequently, S is determined by 
the eigenvalue function available in Matrix-x. This calculation is performed within the 
execution program shown in Appendix A. _ 

The next step is to test the control law via simulation by adjusting the values of a 
and the poles to obtain the desired control characteristics. The Matrix-x system build 
representation of the heading controller that was used for simulation is shown in Figure 
4. Notice that the satsgn function has been modified. Its slope has been adjusted to 
‘provide a filter for numerical chattering. The saturation limit of the satsgn function 
represents the boundary layer thickness @ as developed by Slotine [Ref. 5]. Since the 
range of values of the satsgn function is between -1 and +1,a multiplication factor of 1/o 
must also be included. 

Selection of the boundary layer thickness is a design issue. In Figure 4, the value 
is set at .1 based on many simulation runs. This in turn requires a multiplication factor 


of 10 to be included in the block diagram. 


E. SIMULATION RESULTS 

The goal for the controller was to reach the desired heading and speed in the 
shortest time possible and avoid inefficient use of the control inputs while doing so. Many | 
simulation runs were conducted to obtain the best values for 1, 1),, and the poles for 


the dynamics of the sliding surface of SISO2. In addition, reasonable values for the 
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boundary layer thicknesses and for the satsgn functions had to be determined. 

The output from the simulation is displayed in a set of six graphs. The graph of 
Sliding Coefficient vs Time gives insight into the role that the nN and the choice of poles 
play in minimizing the amount of time it takes to reach the desired heading. The value 
of n primarily affects the time it takes to reach the sliding plane where o(x) = 0. The 
poles that are selected also effect the time it takes to reach the sliding plane by effecting 
its slope but their primary influence is on the dynamics of the system as it travels along 
the sliding plane to reach the desired speed and heading. This shows the need to balance 
the value of 1 and the poles to obtain the optimal behavior for the control system. It 
appears as though a threshold for 7 exists above which the time to reach the sliding plane 
no longer decreases and the only advantage that is gained by increasing 1, would be to 
increase the robustness of the controller as it tries to maintain the system on the sliding 
plane in the presence of unmodeled dynamics and disturbances. Recalling that no 
uncertainties aed been built into this model yet, the choice of the best n values is based 
on response time alone. Initial runs caused a value of .6 to be selected for n,’ and .4 for 
1,”. Using these values additional simulations revealed the best choice of poles for SISO2. 
Heading control was attained with a pole selection of [0,.41,.42]. A more detailed 
description of the effects of each of ste elements of a sliding mode controller along 
with simulation runs to reveal their effects appears in [Ref. 1]. 

This control law was used for the simulation run shown in Figure 5. The AUV was 
initially at rest and commanded to attain a constant speed of 4 ft/sec. While the speed 


controller was accomplishing that task the heading controller was independently changing 
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the heading to 90 degrees from an initial value of zero. The results show a good response 
with efficient use of the control inputs. These results are typical of the many trials that 
were run to verify the control laws, The next step is to apply these control laws based on 
a simplified, linear model, to a more realistic nonlinear model to see if they can still be 


effective in controlling speed and heading. 
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Ii. CONTROLLING THE NONLINEAR AUV 


A. INTRODUCTION 

Two main questions are addressed in this chapter. First, will the sliding mode 
control laws for speed and heading developed in Chapter 2 using a simplified linear 
model for the AUV be able to control a more realistic nonlinear model of the AUV. The 
nonlinear model that is used was originally developed by Boncal [Ref. 2], based on the 
Mk 9 swimmer delivery vehicle and most recently used by Joo-No Sur [Ref. 1] in his 
design of a SMC depth controller. It is in the form of a Fortran code and is shown in 
Appendix B. | 

The second question to be answered is whether the control laws for speed and 
heading control can be simply inserted into the Fortran model as two independent 
modules and work harmoniously with the depth controller already present. If this is 
possible then the task of three dimensional control of the nonlinear six degree of freedom 
model can be accomplished by 3 simple SISO systems rather than a complicated MIMO 
system. Recalling that many simplifying assumptions were made to develop the linear 
model that are no longer valid for the nonlinear model and that some of the assumptions 
| for one of the SISO systems may be in direct conflict with the others, it is likely that 


significant unmodeled behavior will have to be handled by the nonlinear switching terms | 


in the SMC control laws. 


B. SPEED CONTROL [SISO1] 

Recall from Chapter 2 that the best value of 1,’ was .6. Utilizing Matrix-x to get 
the necessary coefficients, the speed control law was obtained and programmed into the 
Fortran code as shown in Figure 6. Notice the use of IF THEN statements to provide the 


satsgn function and to establish an RPM limit. 


ke kKAKAKRPM INPUT CALCULATION *#84# RK AR 
SS1=<U-UD 
IF(ABS(SS1) .LT. 1.0) SATSGN1=(SS1/1) 


IF(SS1 .LE. -1.0) SATSGN1=-1.0 
IF(SS1 .GE. 1.0) SATSGN1=1.0 
RPM=-1153.9*SATSGNI + 83.33*U 
IF (RPM .GE. 500.0) RPM= 500.0 
IF (RPM .LE. -500.0) RPM=-500.0 


Figure 6. Sliding Mode Speed Control Fortran Code 


C. HEADING CONTROL [SISO2] 
The Matrix-x simulations provided the necessary values for the control law. Recall 
that a value of .4 was chosen for 1,’ and that the pole location of [0, -.41,-.42] was 
| selected. This resulted in the following heading control law shown in Figure 7 that was | 
programmed into the Fortran model. Notice that the maximum rudder deflection has been 


limited to + .4 radians. 
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k**k*K*XRUDDER INPUT CALCULATION*#****%** 
§S2=.5732*(PSI-HD) — .0739*V + .8161%*R 
IF(ABS(SS2) .LT. .1) SATSGN2=(SS2/.10) 
IF(SS2 .LE. -.1) SATSGN2=-1.0 
IF(SS2 .GE. .1) SATSGN2=1.0 


DR=2.5785*SATSGN2 + .0328*V + .1112*R 
IF (DR .GE. 0.4) DR = 0.4 
IF (DR .LE. - 0.4) DR = -0.4 


Figure 7. Sliding Mode Heading Control Fortran Code 


D. SIMULATION 
With these two primary modifications to the Fortran model, the same maneuver that 

was shown in Chapter 2 using the linear model was now attempted for the nonlinear - 
AUV. The control laws were very successful at achieving the desired speed and heading 
as can be seen by Figure 8. The effects of the unmodeled terms were present, but 
stabilized by the nonlinear switching term in the control laws confirming the robust nature 
‘of sliding mode control that make it particularly desirable for this application. The 
response was more sluggish and more control effort was required than for the linear 
model. This was expected however since the nonlinear model contains terms to represent 

. the increased loss in a turn which the linear model did not. A comparison to Figure 5, in | 
Chapter 2, shows that it took about 15 seconds longer to reach the desired speed and 


almost 30 seconds longer to reach the desired heading. These results seem very 
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Figure 8. Simulation Results for Nonlinear AUV 
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satisfactory since the use of the motor and rudder inputs were at their maximum values 
throughout most of the run and consequently utilized to their full capacity to reach the 
desired conditions. 

To answer the second question the same maneuver was commanded with a depth 
change of 20 feet included. It should again be pointed out that the depth control law was 
avcioned independently by Joo-No Sur and used without any modification [Ref. 1]. The 
results are shown in Figures 9 and 10. Notice that the depth control law accomplished 
the commanded depth change independent of the course and speed adjustments. The 
control effort of the dive planes seems very efficient as does the use of the rudder and 
motor RPM. All three of these control laws are acting simultaneously and independently 
but together they produce the desired result. | 

To further demonstrate the independence of these 3 SISO systems, two additional 
runs are shown. First, the 20 foot depth change is commanded without requiring a change 
in heading. The depth enane characteristics are then compared to those for the previous 
simulation in Figure 11. The left column is the run with a depth change only and the 
right column shows the depth change while the vehicle is also making a 90 degree 
heading correction. Notice that the results are almost identical showing that depth control 
is not adversely affected by a change in heading. 

Similarly, a 90 degree turn was commanded without requiring a depth change and 
compared to the combined diving and turning maneuver. The results are compared in 


Figure 12. The left column represents the tum only while the right column shows the 


same turn during a 20 foot depth change. Again the results are nearly identical showing 
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Figure 9. Simulation Including Depth Change (Nonlinear AUV) 
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Figure 10. Simulation Including Depth Change (Nonlinear AUV) 
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Figure 12. Effect of Depth Changes on Heading Control 


that heading control is not adversely affected by a simultaneous change in depth. 


E. CONCLUSION 

In wee of these results it can be concluded that the desired three dimensional 
control of the full six degree of freedom nonlinear vehicle can be accomplished by 3 
SISO systems thus avoiding the need to model the AUV as a MIMO system. 

Some problems still remain however that will be addressed in subsequent chapters. 
First, the control of heading is based on assuming full state feedback which includes sway 
velocity. To be realistic, since sway velocity is not directly measurable, an observer will 
have to be designed. In addition, a more global objective is to ultimately cause the AUV 
to travel to a desired way point rather than simply achieving a desired heading. Though 
our approach of developing the control laws as separate SISO systems has been validated 
at this point, it still remains to develop a guidance scheme that can interface with these 
control laws to achieve path following in the horizontal plane. 


The design of the sway velocity observer is dealt with in Chapter 4 and the issue 


of guidance in Chapter 5. 


IV. SWAY VELOCITY OBSERVER DESIGN 


A. INTRODUCTION 

The heading control law as presented in the previous chapters is based on full state 
feedback. In reality, the sway velocity (v) will not be measured thus requiring an observer © 
to be designed that can estimate its value. The approach that will be taken is to utilize 
observer theory to obtain the necessary equations for a reduced order observer and then 
use the Matrix-x software program to obtain the necessary coefficients for these equations. 
The depth control law as shown for the runs thus far utilized a full order ‘ice [Ref. 
1]. Since the speed control law must be based on full state feedback, sway velocity is the 


only state variable which requires an observer at this point. 


B. OBSERVER DESIGN FOR LINEAR AUV 
The state space equations for SISO2 is given in Chapter 2 by Equation 2.9. The 
State space vector is rearranged to separate the measured from the unmeasured states 


resulting in equation (4.1). 
z 0 dea} ry de, 
Wi ={2 0 oO} fH] +] 0] 8, (4.1) 
Vv d,, ¢) d, V, d,,; 


To develop the reduced order observer, the state space matrices will be partitioned 


as shown in equation (4.1) and rewritten as follows. 


r Ao,, AO,, 
Vv = 
Vv AO,, Az, 


- zo 
3 (4.2) 
i) - Ge 
The output system of equations is then written in the form Y=[C]X as shown below. 


10 oO; (2 
(4.3 
so ell | ) 


00 = ()- 


Partitioning the C matrix and the state vector Xo to separate the measured and 


unmeasured states gives: 


[IQ] ={2] (X,) = (3) (X52) = (v) (4.4) 


At this point the relationship shown in reduced order observer theory can be 
systematically applied to complete the observer design [Ref. 8]. Note, X,, represents the 
measured state variables and X,, is the sway velocity that is to be observed. 


Consequently, the following equation is obtained. 


Rone 0 = (EE 22) (4.5) 


In order to better understand the following relationships that are used to find [LL] 


and [ZZ], a block diagram is of the observer is shown in Figure 13. 


Figure 13. Sway Velocity Observer ( Initial Design ) 


The values of [FF], [GG], and [H] are given by the following equations. 


[FF] = [Ao,,) - [LL] [C,} [Ao,,] (4.6) 
[H] = [Bo,] - [LL] [c,] [Bo,] (4.7) 


[GG] = ( [Ao,,] - [LL] [¢,] [Ao,,) ) le)” (4.8) 


It will be necessary to consider [LL] and [GG] to have two parts resulting in 
[LL] = [I,l,] and [GG] = [gg,,gg,]. Upon substituting into the above relationships the 


following equations are obtained. 


FF =d,, - 1, d, (4.9) 


H=d,,- 1, dy, (4.10) 
99, = a, - 1, d&, - I, 99, = 0 (4.12) 


Notice that the above values are all scalars and that [LL] is the only part of the 
observer equation yet to be determined. [LL] will be found using pole placement as 


follows: 


| S-FF|=|S-d, - 1, d, | (4.12) 


Since |, is not included in equation 12 it can be considered arbitrary and assigned 


a value of zero. The resulting equation for 1, = [LL] is: 


1, = (-FF + d,, )/d, (4.13) 


It is at this point that Matrix-x was used to determine the coefficients necessary to — 


calculate the observed value of sway velocity. In addition simulations were conducted to 


compare the observed value of sway velocity to the actual value. The best results were 


obtained by choosing FF to have a value of -2 giving the following equation. 
V=el,r+ ZZ (4.14) 
Where: a 


2Z =H’, +99, 01+ FF VO ° 


FF = -2 . gg, = -57.827 H = -15.575 I, = -104.850 


The Matrix-x system build block diagram which was used for the simulation is 
shown in Figure 14 and the entire autopilot with the observer included is shown in Figure 


15. 


Figure 14. Reduced Order Sway Velocity Observer 
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Figure 15. Autopilot With Sway Velocity Observer 


The same maneuver simulated in Chapter 2 and shown in Figure 5 was again 
"simulated using the estimated sway velocity. A comparison of sway velocity and its 
observed value is given by Figure 16. The observed value is exactly the same as the 


actual showing that the observer has been designed properly. The rest of the run is 


39. 


included as Figure 17 confirming that the autopilot with the reduced order observer is 
operating properly. The next step is to include the sway velocity observer into the Fortran 


code representing the nonlinear AUV. 


o 5 10 15 20 2s 30 35 40 as s0 
TIME [SEC.] 


. Figure 16. Comparison of Modeled vs Observed Sway Velocity 
(Linear AUV) 
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C. APPLICATION TO NONLINEAR AUV 

The initial attempt to use the observer equation was unsuccessful when applied to 
the nonlinear AUV. The clue to the problem came by noticing that the only time the 
observer worked well was if the initial value for the surge velocity (u) and the command 
speed were both 6 ise (the nominal linearization speed). To overcome this problem and 
allow for a more accurate estimate of the sway velocity throughout its range of operation 
the Matrix-x model of the AUV was linearized around a variety of speeds and the various 
coefficients were recorded. Curve fitting techniques were then employed to relate these 
coefficients to the surge velocity (u) so that the nonlinearities actually present in the real 
vehicle could be better accounted for. The resulting values for the coefficients are shown 
in fe gas D along with the equations which resulted. These equations were then 
saciacds in the Fortran code and the same simulation run was repeated with the improved 
adaptive observer providing the estimate of sway velocity. The results are shown in 
Figures 18 and 19. From the simulation it is clear that the control laws continued to work 
well. 

The difference between the observed value and the actual value are shown 
graphically in Figure 20. This figure shows that the observed value is a fair approximation 
for v allowing accurate control of the vehicle. The accuracy could possibly have been 
improved by linearizing around more speeds to get a better set of equations for the — 
coefficients but since the value of v is so small the impact of assuming it to be zero was 


explored prior to attempting to refine the observer any further. 
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Figure 20. Comparison of Modeled vs Observed Sway Velocity (Nonlinear AUV) 


D. EFFECT OF ASSUMING SWAY VELOCITY NEGLIGIBLE 
Figures 21 and 22 show the effect of setting the sway velocity equal to zero and 


commanding the same tum and speed increase as shown in Figure 16. The performance 
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of the heading control law is virtually the same! It is a tribute to the robustness of sliding 
_ mode control that sway velocity is best handled as an unmodeled disturbance than an 


observed quantity. 


E. CONCLUSION 

In view of these results, the autopilot will be designed upon the assumption that the 
sway velocity equals zero. This will simplify the autopilot and increase the speed at which 
the calculations are performed. Consequently, the problem of agioving the observer 
design is eliminated and SISO2 is effectively reduced to a two state feedback system. 
This decision may have to be reevaluated if disturbances or currents are added to the 
model that cause the sway velocity to vary more than can be handled by the nonlinear 


switching term in the heading control law. 
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Figure 21. Sway Velocity Set Equal to Zero (Nonlinear AUV) 
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V. GUIDANCE 


A. INTRODUCTION 

The guidance portion of an AUV is the link between the mission planner and the 
autopilot. The mission planner decides the coordinates of the way points the vehicle must 
travel to and the vehicle’s speed. The autopilot, as explained in the previous chapters, 
requires inputs of desired heading, depth, and speed. Though speed and depth can be 
given directly to the autopilot, a guidance scheme that will convert the x and y 
coordinates of the way point into the required heading command must be developed. The 
method of guidance that was selected is called Line of Sight Guidance (LOS). In this 
chapter, the basics of LOS are explained and the critical aspects of an autopilot based on 


LOS are examined. 


B. LINE OF SIGHT GUIDANCE 


1. Fundamental Relationships 
Line of Sight Guidance determines the desired heading by using the vehicles 
cuttent position and the coordinates of the next way point. As its name implies, LOS 
essentially points the AUV at the next way point along the line of sight no matter where 
the vehicle is located. Notice that some sensors, for example an inertial navigation system, 
must be available to determine the current position of the AUV. Figure 23 shows how 


the desired heading (y,) can be simply obtained from the coordinates of the current 
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position (x,y) and the coordinates of the next way point (xyy,) by using the ATAN 


function. 


Yd = ATAN [(Yd-Y)/(Xd-X)] 


Figure 23. Finding the Desired Heading 


Notice that in Figure 23 a tum to port is the obvious choice but if the next 
_ way point had been in the fourth quadrant a tum to starboard would have been more 

efficient. This shows the need to compare which tum will allow the desired heading to 
ieachad with the least amount of course correction. To understand how this was done, it 


should be pointed out that y, will have a range of values -<wy,<m. To account for the 
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cases when w, is negative, 2 was added to w, to give W,,. Next, a comparison of the 
absolute value of the angular difference between the desired and the actual heading was 
made. The choice of y, or W,, was based upon which of these gave the smaller course 
correction. This value that was called Wam and was value given to the heading control 
law as the desired heading. Finally, it should be pointed out, that if the term (y,,,-) in 
the heading control law is positive a tum to port will result. Consequently, if it is negative 
the AUV will tum to starboard. An illustration of the selection of the best turn direction 


is outlined in Figure 24. 


| Ya-¥|=3 1/2 
| Ydp - ¥ |= tr/2 


Shortest 7 
Turn , Yam =5 71/4 


Yam-Y = T/2 


+ => Turn to Port 


Figure 24. Choosing the Smallest Heading Correction 


2. Interface parameters effecting performance 
There are several factors that impact the equation that determines the desired 


heading and consequently the effectiveness of the system as a whole. These factors are 


51 


discussed at this point so that the results of the secant sinmulations can better be 
appreciated. , | 

First, how often does the autopilot receive si update as to its current position 
from the navigational instrumentation on board. This will effect the accuracy of the 


calculation of wy as will the accuracy of the navigational instrumentation itself. 


Next, a criteria must be established for considering the AUV to be considered 


close enough to the desired way point that the next way point should be provided by the 
mission planner. This has the effect of aiming at a target with the desired way point at 
its center and will account for the advance and transfer of the AUV. The dimensions that 
are established for this "zone of acceptability" will directly affect the amount by which 
the AUV overshoots one tag point on its way to the next. 

The turning and diving characteristics of the AUV must also be considered. 
A way point must be far enough away that the AUV can physically maneuver to reach 
it. On the other hand, the more frequently the eas points are specified the more control 
the mission planner can exert on where the AUV is while in transit between two way 
points. 

Finally, the environment the AUV is in must be considered. Factors such as 

currents can greatly affect the accuracy with which the AUV can be controlled. The 

| number of obstacles that must be avoided and hence the greater need to prescribe a path 
rather than just a way point will also be an important factor to consider. 

Keeping all these factors in mind, it is clear that the mission planner, guidance 


scheme, and autopilot must ultimately be evaluated as a system with a particular mission 
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rather than three independent functions. It is in this context that the following results are 


presented. 


3. Results 
Run 1 shows the versatility of the system using LOS guidance under the 


following nearly ideal conditions. 


* (x,y,z) position updates are accurate and are provided every .1 sec. 


¢ Way points are distant enough to be well within the turning and diving capabilities 
of the vehicle. 


¢ The target radius is one shiplength. 


¢ No disturbances or obstacles. 


The mission that is envisioned is that of a straight line transit followed by a 
slow speed search. It will require both port and starboard turns and increases and 
decreases in both depth and speed. The way points are identified in the horizontal plane 
by crosshairs and the desired depth and speed are represented by dotted lines. Figure 25 
shows the results of the simulation. The AUV went to each way point that was specified 
and did so at the required speed. The control action of the rudder , dive planes, and motor 


were very efficient. This run has to be considered to be highly successful thus proving 


LOS to be a valid guidance method. Notice that the mission ends with the AUV one 


shiplength away from the last way point waiting for the new way point to be provided — 
by the planner. 


The first parameter that was changed was the time between updates of the 
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Figure 25. RUN 1, LOS Guidance for the Nonlinear AUV 


actual position of the AUV. The first signs of a decline in performance became evident — 
when the update time reached 5 seconds. The results are shown in Figure 26. The control 
was still very successful which shows how tolerant LOS guidance is to inaccuracies in 
determining the AUV’s position. Notice however that there is an increase in the control 
effort for the rudder. The fluctuations that result whenever the update is received are not 
optimal so it is recommended that the update time be kept below 5 seconds. 

The update time and the accuracy with which the AUV’s position can be 
determined will become more and more important as the way points become more closely 
spaced in an effort to achieve even greater control of the transit path. Notice that these 
way points are approximately ten shiplengths apart. 

The next simulation runs explored the effect of the size of the target radius. 
The optimal target radius was determined to be two shiplengths. The improvement that 
a two shiplength target radius resulted in as compared to a target radius of one shiplength 
can be seen by comparing Figures 25 and 27. Since the next way point is provided one 
shiplength sooner, the control action required to reach it can be initiated earlier. The old 
way point is still reached by virtue of the momentum of the AUV but the amount by 
which the AUV overshoots is decreased. This reduces the total path length and 
consequently improves the overall performance. A target radius of two shiplengths seems 
to work best since increasing it any further would result in missing the original way point 
by too great a margin as the AUV redirects itself toward the newly assigned way point. 

The effect of the frequency with which the way points are given was explored by 


the next simulation run. In an attempt to improve the performance of the AUV two 
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Figure 26. RUN 2, LOS Guidance (Update Every 50 Samples) 


additional way points were included in between those originally provided. This was done 
in an attempt to cause the AUV to take more of a straight line path to the next way point. 
The additional way points were in line with the original two and were located about five 
shiplengths away. Five shiplengths is the turning diameter for this vehicle so the idea was 
to get a maximum turning effort initially and then follow a straight line for the second 
part of the path. The target radius for this run remained two ship lengths but the position 
update was I second since the way points are ser together. Figure 28 shows the results. 


A greater degree of control of the AUV is attained and a shorter distance of travel is 


required. The improvement can be easily seen by comparing Figures 27 and 28. 
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Figure 27. RUN 3, LOS Guidance (Target Radius = 2 SL) 
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Figure 28. RUN 4, LOS Guidance (Additional Way Points) 


VI. HIGH SPEED APPLICATIONS 


-A. INTRODUCTION 

The design of the autopilot and LOS Guidance system is based on a nominal 
spotting speed of 6 ft/sec. This chase explores the capability of the autopilot and 
guidance system to successfully control the AUV during high speed operations. The goal 
is to control the AUV at speeds as high as 50 ft/sec (30 knots) and as low as 3 ft/sec. 
This wide range of operating speeds will result in an extremely large change in the 
vehicle’s hydrodynamic coefficients. Recalling that the control is based on a model that 
was linearized around a speed of 6 ft/sec, it becomes clear that this will be a severe test 
of the robustness of the sliding mode control laws. 

To better appreciate the extent of the unmodeled behavior, graphs of pitch and pitch 
rate are included in the simulation results. Recall that they were both considered to be 
zero during the simplification of the equations of motion for SISO2. 

As was mentioned previously, the depth control law has been based on observed 
values of pitch and pitch rate up to this point. This is advantageous for a small vehicle 
that does not have enough room for the instrumentation necessary to measure these 
values. For an AUV capable of a 30 knot operating speed however, it is assumed that 


there will be instrumentation onboard to provide these measurements. Consequently, the 


simulation will be done assuming full state feedback with the exception of the sway 


velocity. Sway velocity will still be given a value of zero and handled as a disturbance 


as was explained in Chapter 4. 


: B. 


AUTOPILOT SIMULATION RESULTS 


The first set of simulation runs test the ability of the autopilot to achieve the desired 


heading, speed, and depth. The capability of the LOS guidance system will be 


demonstrated later once the autopilot has been verified. 


1. Runil 


The conditions for Run 1 are as follows: 


Command speed 
Command depth 
Command heading 


Integration time step 


18 FPS. 
2 Shiplengths 


90 Degrees 


.01 Sec. 


This run tested the ability of the autopilot to operate at a speed three times 


greater than it was designed for. Before satisfactory results could be attained at higher 


speeds, the integration time step had to be decreased. The relationship between the surge 


velocity and the integration time step is found by expressing it in a nondimensional time 


as shown in Equation 6.1. 


NDat = u at/L (6.1) 
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This relationship shows that if the nondimensional time step is to remain 
constant, the time step used within the simulation must decrease as the speed increases 
to obtain the correct results. By referring ‘back to the initial work at slow speeds, the 
value for the nondimensional time step was found to be .1. Based upon Equation 6.1, an 
integration time step of .01 sec was chosen for the surge velocity of 18 ft/sec. 

The results as shown in Figures 29 and 30 are outstanding. The autopilot operates even. 
better at high speed. All desired values are attained with a minimal amount of conte 
effort and with no modification of the control laws. The autopilot is now ae to be 


verified using the boundaries of its design envelope. 


2. Run2 
The integration time step was decreased to .005 seconds for this run at 50 
ft/sec to be consistent with keeping the nondimensional time step eaaitane In addition the 
Auv is given an initial surge velocity of 50 ft/sec rather than starting from rest to show 
the effect of a high speed turn. The remaining characteristics of this run are the same as 
Run 1. The results are shown in Figures 31 and 32. 
Again the results are very good with no problems evident. This result is a most 


impressive verification of the robustness of sliding mode control. 


3. Run3 

Run 3 is included to show that the autopilot is also capable of slow speed | 
operations at 3 ft/sec. An integration time step of A seconds is adequate for this speed. 
Figures 33 and 34 show that the AUV is more sluggish at the slower speed but the 


control laws have no problem attaining the commanded heading, speed, and depth. 
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Figure 29. Run 1, High Speed Maneuver (18 ft/sec) 
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Figure 32. Run 2, Maximum Speed Maneuver (50 ft/sec) 
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Figure 33. Run 3, Slow Speed Maneuver (3 ft/sec) 
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Figure 34. Run 3, Slow Speed Maneuver (3 ft/sec) 
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4. Conclusion 
The autopilot has been verified to have the robustness necessary to operate 
within a speed range of 3 ft/sec to 50 ft/sec. The integration time step can be set at .005 
sec for all values of operating speed or it can be related to a constant nondimensional 
time step of .1 using Equation 6.1. The advantage of varying the time step is that the 
calculations are performed faster for the slow speed runs wheic the time step can have 


a larger value. 


C. LOS GUIDANCE SIMULATIONS 

The final link in establishing that the AUV is ready for high speed operations is to 
show that LOS Guidance can still steer it through the desired way points at a speed of 
50 ft/sec. This will be demonstrated by commanding the same basic maneuver performed 
in Chapter 5. To decrease the time needed to accelerate to the new higher speeds, the 
simulation will start with the AUV already at 50 ft/sec when the first way point is 


provided. The way points themselves will be further apart as is appropriate for high speed 


travel. 
1. Run4 
The conditions for Run 4 were as follows: 
* Commanded speed 50 Ft/sec. 
* Integration time step 005 Sec. 
¢ Target radius 2 Shiplengths 
° Navigation update time .005 Sec. 
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The results of the run are shown in Figure 35. Though the AUV passed 
through all of the way points, the control of speed is not satisfactory. In exploring the 
speed control law for SISO1, it was determined that the nonlinear switching term was not 
strong enough to call for the necessary motor rpm to maintain the speed characteristics 


on the sliding surface. 


2. Run 5 
For Run 5 the value of the coefficient of the switching term was increased to 
4000 thus enabling the switching term itself to call for enough rpm to result in a 50 ft/sec 


speed. With this modification, the speed control law for SISO1 becomes: 


6.2 
RPM = -4000.0 satagni + 83.33 u ( ) 


With this new speed control law in place, the same maneuver was again 
commanded. The results are shown in Figure 36. The control of speed is now much better 
and the AUV still passes through the desired way points. Notice that when the desired 
speed is not attained, the control now calls for the maximum effort, which is 5000 rpm, 
from the propulsion motor. In order to improve speed control any further, a larger 


propulsion system would have to be utilized. 


3. Run 6 

This run explores the maximum Navigation update time interval that can be | 

tolerated at high speeds before a deci in performance is detected. Various runs were 
conducted with the first signs of trouble occurring when the navigation update time was 


1 sec. The results for this run are shown in Figure 37. The need for a shorter Navigation 
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Figure 36. Run 5, High Speed, Multiple Way Point Maneuver (New Control Law) 


update time is to be expected since the AUV travels further at high speeds during the 
update interval thus causing it to be further away from the last known position used by 


the control law. 


4. Conclusion 

LOS Guidance works as well at 50 ft/sec as it did at 3 ft/sec for the runs 
shown in Chapter 5. The only modifications that were required were to increase the 
strength of the switching term in the speed control law and decrease the integration time 
step and navigation update time. The fact that only the speed control law had to be 
modified is consistent with the fact that the control surfaces become more effective at 
higher speeds resulting in ae response to a given control signal. To increase the speed, 
on the other hand, requires more motor rpm thus causing the speed control law to be 


modified consistent with the larger propulsion system that would have to be installed to 


achieve the higher operating speed. 
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Figure 37. Run 6, Effect of Increasing Nav Update Time 


74 


Vl. EFFECT OF CURRENT ON LOS GUIDANCE 


A. INTRODUCTION 

This chapter investigates the effect of current on the autopilot’s ability to reach a 
series of way points using LOS Guidance. The speed during these runs will be 4 ft/sec 
which is consistent with the operating speed of the AUV under construction at NPS. The 
first runs attempt to reach a single way point with the current coming from various 
directions. The next simulations will show the effect of the strength of the current. The 
last set of simulations show how the presence of a strong current can effect P path made 


up of multiple way points. 


B. EFFECT OF CURRENT DIRECTION 
1. Runl 

To see just the effect of the direction of the current, a single way point at 
(10,10,0) was chosen and a constant speed of 4 ft/sec was commanded for the AUV. 
Figure 38 shows the path that resulted when no current was present. Notice that a new 
graph of the distance the AUV is away from the next way point (DAWAY) has been 
added. The target radius was set at .01 shiplengths to allow the vehicle to get as close as 
possible to the prescribed way point. This represents the standard to which all other runs - 


can be compared. 
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2. Run 2 and Run 3 
These runs, shown in Figure 39, demonstrate the effect of a 2 ft/sec current. 
This current is 50 % of the operating speed of the AUV which should be considered to 
be an extreme case. Both runs are shown in Figure 39. For Run 2 the current was from 
left to right and for Run 3 the current was right to left. Notice that the AUV still reaches 
the way point in both cases but is swept away from the no current path. Notice too that 


in both cases the AUV has a final approach which is against the current. 


3. Run 4 and Run 5 
For Run 4 the current direction was from top to bottom. For Run 5 it was 
from bottom to top. In both cases the final approach once again is against the current, see 
Figure 40. In addition the miss distance was the largest when the current comes from 
bottom to top. It should be remembered however, that the target radius that will be used 
during the actual operation is two shiplengthis. Since the AUV wis able to get within 


approximately 1.5 shiplengths, this run should be considered a hit also. 


4. Conchision 
The AUV will reach the prescribed way point under extremely adverse current 
conditions. The problem however is that its path deviates from the near straight line path 
of the no current case to the point shal it may be unsuitable for operation in an 
environment which requires a high degree of control over the path. The other problem to — 
address is that the AUV will approach the way point against the current which may be 


undesirable considering the position of the next way point along its path. 
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C. ADAPTATION OF STEERING CONTROL 
For the runs shown in Figures 39 and 40 the value for the x and y position of the 


AUV are calculated by the following equations where the subscript c denotes current. 


X =u, + ucosp - v siny 
(7.1) 
yev.,+usinp + vsiny - 
Recall too that the desired heading is calculated using Equation 7.2. 
Wa = ATAN[ (y-yd) / (x-xd) ] (7.2) 


Recall that at steady state the desired heading and the actual heading are equal. Therefore, 
_ at steady state, with a desired way point at (0,0) for simplicity, Equation (7.2) can be 
substituted into Equation (7.1) which upon integration yields the result shown as Equation 


(7.3). 


Ya = ATAN (v,/u,) | (7.3) 


Equation (7.3) shows that the desired heading at steady state ultimately is parallel to the 
direction of the current. This result is verified by the simulation results shown in Figures 
39 and 40. Notice that if the vertical component of the current is zero as is the case for 
Figure 39 the desired heading becomes 0 or 180 degrees. Correspondingly, if the . 


horizontal component of the current is zero, the desired heading is + or - 90 degrees as 


shown in Figure 40. 


Once an understanding of the above relationships was attained, the next step was 
to modify the control law to attain tighter control over path. This was done by specifying 
a desired heading that compensates for the direction of the current rather than being 
controlled by it. To do this, recall that the equation for the sliding surface for SISO2 is 


given by Equation (2.16) which is repeated here as follows: 


0 = 8 (P - Py) + 8, (v) + 8; (r) (7.4) 


By considering the simple case where the AUV is commanded to travel along the x axis, 
a modification to the sliding surface can be attained. For this simple path the desired 
heading is zero. In addition the speed in the y direction must also be zero. This 
observation coupled with Equation (7.1) gives the relationship shown in Equation (7.5) 
that can be used to find the steady state error that must exist to compensate for the effect 


of the current. | 


W, = - ASIN(v,/u) . | (7.5) 


This result requires that the actual heading of the AUV be composed of two parts as 


shown in Equation (7.6). 


= ¥e+ hy (7.6) 


Recalling that the sway velocity v has been estimated to be zero and that r is zero at 
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steady state, the sliding surface equation shown in Equation (7.4) becomes: 


Oo=s, HN = 5, V, 


Notice that the sliding surface equation will not be zero at steady state unless another 
term equal in magnitude but opposite in sign is added to the sliding surface equation. It — 
is by this reasoning that the equation for the sliding surface for SISO2 was modified to 


become: 


Oo = s,(W-Wy) + SV + S,r +S,ASIN(V,/u) 


This additional term has the effect of introducing a steady state error for heading that 
exactly offsets the effects of the current and improves the path keeping characteristics of 
the AUV. It should be pointed out that this equation relies on perfect knowledge of the 
current velit allowing the ebeaponient of the aidents velocity that is perpendicular to 
the AUV’s straight line path between consecutive way points to be determined. 

The improvement which results is shown by rerunning the same simulations 
discussed at the beginning of this chapter using the modified sliding surface for SISO2. 
The improvement is easily seen in Figures 41,42,43,and 44 where the old results are on 
the left and the new results are on the right. Based upon this comparison, it can be 
concluded that LOS Giidaieeccan be highly successful at controlling an AUV in a high 


current environment if the velocity of the current is well known. This shifts the burden 


of tight control over the path to the design of an accurate estimator for current velocity. 


Such an observer will not be included in this work but the following simulation is 
included to give an indication of the impact an inaccurate estimate of current would have. 
The current was given a value of 2 ft/sec from bottom to top. The estimated value was 
1 ft/sec from bottom to top. Figure 45 shows that the result is still improved from the 
case where no compensation for current was included. From this it can be concluded that 
even an imperfect estimate of the currents velocity will result in some improvement in 


the AUV’s path. The better the estimate the more improvement that results. 


D. MULTIPLE WAY POINT PATHS 

The ultimate test for the autopilot using LOS Guidance is to show that it can reach 
a series of way points. The same path used through out this work is repeated here one last 
time to demonstrate that the AUV will travel through a series of way points even in the 
presence of a current that has a magnitude of 1.4 ft/sec in a direction 45 degrees away 
from the origin. The result is shown in Figure 46 which can be compared to the same run 
done without a current present in Figure 47. The results show that LOS Guidance based 
on a heading control autopilot that has been modified to account for ‘i presence of a 


current is very capable of guiding the AUV through the prescribed way points. 
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VII. CONCLUSIONS AND RECOMMENDATIONS 


A. CONCLUSIONS 
“ . The following are the major conclusions that can be drawn from this study. 


¢ The control laws for speed, heading, and depth were all developed independently, 
and yet their individual control actions did not interfere with each other. 


¢ The control laws, based on a simple linear model, were able to control the more 
complex nonlinear model of the SDV MARK 9 vehicle by virtue of the nonlinear 
switching terms, associated with Sliding Mode Theory, that compensated for any 
unmodeled behavior and enhanced robustness. 


* In developing the steering control autopilot, sway velocity is best handled as a 
disturbance rather than an observed quantity. 


¢ LOS Guidance provides an effective means of interpreting the position coordinates 
of the way points provided by the mission planner into commands for heading that 
can be executed by the autopilot. An inherent characteristic of LOS Guidance is that 
it enhances the robustness of the system as a whole. In addition, tighter control of 
path can be attained by specifying the way points more frequently. 


¢ The robustness of this combination of Sliding Mode Control and LOS Guidance is 
demonstrated by the 3 ft/sec to 50 ft/sec speed envelope over which the autopilot 
operated effectively even though the linearization that produced the simple linear 
model upon which the control laws were based was done about a nominal operating 
speed of 6 ft/sec. 


¢ LOS Guidance can be easily modified to take advantage of known or observed 
current velocity thus allowing the vehicle to operate in strong currents. 
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B. RECOMMENDATIONS 
The following actions should be taken to fully utilize the research done for this 
thesis. 
* Verify simulation results using the actual AUV currently under construction at NPS. 
¢ Contrast this autopilot using LOS Guidance to other techniques for controlling 


AUVs, such as minimizing cross track error, to ensure the best method is ultimately 
adopted. 


¢ Design an observer that can determine the velocity of the current that the AUV is 
operating in. 


APPENDIX A 


A (RIOR ROR IORI IOI II III III IOI ITO III IOI TORII ROT ITOK RIT OK toe 


//* * 
Jf AUV AUTOPILOT EXEC PROGRAM * 
//* * 
//* THIS PROGRAM CALCULATES ALL VALUES NEED TO SIMULATE * 
//* SPEED AND HEADING CONTROL FOR A LINEAR, FULL STATE * 
//* FEEDBACK, MODEL OF THE AUV. * 
* * 

* 


SRR IRI IK RHI RIKI KIKI RII RIK RRR RAR IIHR HRI KR IRR IK IKK R RRR RIK KIKI KR kk 


//THIS PROGRAM COMPUTES THE S COEFFICIENTS FOR 2 SISO SYSTEMS. 
// heading is entered in degrees and converted to radians in the program 


// KKkaKRKKKKKKKKKKKRKKRK KIER SYMBOL IDENTIFICATION KREKKKKKEKKEKEKKEKK KKK EK 

// wubar= nominal operating speed upon which the linearization is Vasen (ft/sec). 
// Nbar= motor RPM that results in ubar. 

// &G = length of SDV-9 (ft). 

// mass = mass of SDV-9 (slugs). 

// 1% = moment of inertia of the SDV-9 (ft lb sec2). 

// den = fluid density (Slugs/ft3). 

// Hd = desired heading (degrees). 

// wud = desired surge speed (ft/sec). 


[ [RRR Ik Rak Akiak ASSIGNMENT OF CONSTANT VALUES *4 4 ARR RK RK RIK RK 


ubar=6; 
nbar=500; 
L=17.4; 
mass=372; 
Tz=10000; 
den=62.4/32.2; 
kvis=1.082D-5; 


//FLOW PARAMETERS 
Yr=.0297; 
Yv=-.0931; 
Nrd=-.0034; 
Ydr=.0273; 
Yvd=-.0555; 
Nr=-.0164; 
Nv=-.00742; 
Ndr=-.0129; 
Xud=~.0076; 
Nvd=.0012; 
Yrd=.0012; 
CDo=.00385 + 1.296D- 174(ubar#l/kvis -1.2D.-7)**2; 


//** DEFINATION OF SYMBOLS REPRESENTING CONVIENENT GROUPING OF TERMS ** 


ALPHA=2*den*L**2*CDo/(2*mass—den*L**3*Xud) ; 
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// FIFI IK IK KIKI IK KIKI III AIH KI IK IH IKKE KK IIIA KKK KEKKKREKERKEKAKRKKKKKK 


J/ * * 
// * CALCULATION OF A AND B- MATRICIES FOR SISOL WHERE X=[{u]’ * 
// * * 


// FHA III IAHR IK II HHI IIHR KK RIKKI KK KI KIRK IKK KI K EKER KKEKKKEHRKKAKEEKEEKRKKKKKEKKKKK 


Al. = ~ALPHA*ubar; 
Bl = ALPHA*.012**2*nbar; 


// HK IK TK II III III HII IK III IK IK IKK KR KKK KI IK IKARIA KIRK IKKR KKK KK KKK 


lf * DECOUPLING THE LATERAL AND YAW EQUATIONS OF MOTION * 
// PASREENEE EER ARES ASLO E SEE ERE E CRM ne Rane ne Rae pee Oy 
ee Be 0*ones(2,2); 
=0.0*ones(2,3); 


1,1)=mass—den*L**3*Yvd/2; 
1,2)=-den*L**44yrd/2; 
2,1)=-den*L**4*Nvd/2; 
2,2)#I1z—-den*L**5*nrd/2; 
C(1,1)=<den*L**3*yr*ubar/2; 

1,1) =den*L**3*Yr*ubar/2 - mass*ubar; 
1,2)#den*L**2*yveubar/2; 
1,3)=den*L**2*ydrteubar**2/2; 
2,1)=den*L**4*Nr*ubar/2; 
2,2)=den*L**3*Nveubar/2; 

2,3 


M 
M 
M 
M 
/ 
Cc 
Cc 
Cc 
c 
Cc 
Cc J=den*L**3*Ndr*tubar**2/2; 


( 
( 
( 
( 
7 
( 
(1, 
(1, 
(2, 
(2, 
(2, 


// FIFI KI AIH I TIF IAI IAI IR IK IIT IK II II IKK III IK ITI RII THI KIKI KK EKER KEKKEKKKKKEE A 


Vin: x 
' f/f * CALCULATION OF A AND B MATRICIES FOR SISO2 WHERE X=(Heading,v,r]’ * 
/ * * 
TE) SIG ISI UII II SIS IISIIIIIII SII SII ICICI III ISI III CISTI TTT CII IIIT II 


D=inv(M)*C; 


A2=[0,0, ee 


fae! 
B2=[0;d(1,3); 


(1 Nl Ae a 
a(2,3)43 
J[ RRRKKKK KR KERRI CALCULATION OF S1,82,53, S54 KRERKKRRRRERRKERKARKR KA 


inquire atasql 
inquire atasq2 


inquire poles2 


-ke2= poleplace(A2,B2,poles2); 
AC2=A2-B2*kec2; 


[evec2,eval2J=eig(AC2'); 


Sill; 
S2=evec2(1,1); 
S3=evec2(2,1); 
S4-evec2(3,1); 
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// HIKARI KIKI KEIR IIR III KIKI KIKI IKI KEK IKK KH KIK EKER KKK KKEKEKKKKKKEKEKKEK 


/* DETERMINING GAIN VALUES FOR MATRIXx BLOCK DIAGRAM. * 
// HHI II IK HK IK RIK HR IK IMR KI IKK RH KK IKE KEKE KKK KKK KKEKKEKKKKKHKKKEKEAKKKKEK 
// CONVENIENT GROUPING OF TERMS: 

DD1=COUD* .012**2*nbar; 
DD2=S3*d(1,3)+S84*d(2,3); 
Natal=-atasql/DD1; 
Nata2=-atasq2/DD2; 

NN1=S1*A1/DD1; 

NN2=(S3%*d(1,2) + S4*d(2,2))/DD2; 
NN3=(S2+ S3*d(1,1) + S4* d(2,1))/DD2; 


dil-d(1, 


// KKKKEK RIKKI III KIKI KEK RIKI IKI KIRK RAK IKE KE KKK KK KEK REKKEKKEKKREKKEKKKKKEK 


f * SIMULATION x 


// FOI II ISI IIR RG GIGI ISI GIGI GIGI ISIC IGG GOI I IGOR IIIT III I IIR eit 
inquire tsim 

tstep=.1; 

inquire ud 

inguire HdDEG 

Hd = HdDEG*6.28/360; 

T=(O:tstep:tsim]'; 

ud=ud*ones(T); 

Hd=Hd*ones(T); 

U=(ud,Hd]; 

Y = SIM(T,U); 

// HII KHIR HIKARI KAKEKKEKKKAKRKKKKKKKKKKKKK KARR KKKRKKKKK KKK Ka K 
// * GRAPHICAL DISPLAY OF RESULTS * 
Vf KHAKI KER HKKK KEE KK KKK KEKE KK KKK KEK EKER KKKKEKEEKKKKKEEKKKKKEKKRKKKEKMKK 
HHD=(Y¥(:,2)*360/6.28 Hd*360/6.28]; 
uuD=[Y¥(:,1) ud]; 


PLOT(T,Y¥(:,3),'UPPER LEFT XLABEL/T/ YLABEL/SIGMA1/TITLE/ ATASQ1=.6 ... 
BAND=1FPS/' ) 


PLOT(T,Y(:,5),'UPPER RIGHT XLABEL/T/ YLABEL/SIGMA2/TITLE/ATASQ2=.4 POLES... 


«(0,-.4,-.42] BAND=.1RAD./’ ) 


PLOT(T,Y(:,6),*LOWER RIGHT XLABEL/T/ YLABEL/RUDDER/TITLE/ STEERING .. 


(MAX DEFLECTION=.4RAD)/'‘ ) 


° 


PLOT(T,Y(:,4),LOWER LEFT XLABEL/T/ YLABEL/RPM/TITLE/ PROPULSION... 


(MAX. RPM= SOORPM)/') 


PLOT(T,HHD,’UPPER RIGHT XLABEL/T/ YLABEL/H & HD/TITLE/ HEADING ... 
(Hd = 90 DEG)/? ) 
PLOT(T,UUD, ’UPPER LEFT XLABEL/T/ a U & UD/TITLE/ SPEED. ... 


(Ud = 4.0 FPS)/’) 
//exec('RUNSUPERG1.X’ ) 


APPENDIX B. 


PHIRI IK HHH III KI IKI RIKKI KI KKK IKI KKK KKK EKKEKHKKEKEKKKKKKKKKKK ; : = 


NONLINEAR AUV MODEL / STERN PLANE AND BOW PLANE SEPARATED 


* 
k * 
* . * 
* VARIABLE DECLARATION 11 INPUTS 280 = * 
* CONSTANTS 80 PROPULSION MODEL 300 * 
* INITIAL CONDITIONS 136 OUTPUTS 452 
* MASS MATRIX 230 INTEGRATION 475 
* INVERT MATRIX , 268 CONTROL LAWS 500 * 
HHI KI KKK KHAKI KEKE KEKHEKKHEKEKKKEKIKKKKEKKEKEKAKKKEKKEKKKKKK KKK 

REAL AW(82,82) 

REAL MASS, LATYAW, NORPIT 

REAL MM(6,6),G4(4),GK4(4),BR(4),HH(4) 

REAL B(6,6),BB(6,6). 

REAL A(12,12), AA(12,12),INDX(100) 

REAL XPP ,XQQ ,XRR ,XPR 

REAL XUDOT ,XWQ ,XVP ,XVR 

REAL XQDS ,XQDB ,XRDR ,XVV 

REAL XWW ,XVDR ,XWDS. ,XWDB 

REAL XDSDS,XDBDB ,XDRDR ,XQDSN 

REAL XWDSN ,XDSDSN 

REAL TIME,S,EITA,UBAR, UHAT,COMZ,BAR,SIM,DE, SAT, VHAT, ZZ0BS 

REAL SS1, $82,UD,XD, YD, TD, TNWP, XA, YA, HD, HDMDEG, DAWAY, SATSGN1 

REAL SATSGN2,ZZO0BSDOT,, NEARMISS , ALPHA, VCC, XD1, ¥D1,XD2, ¥D2, DY, DX 

REAL NAVUPDATE, TNAV, TARGET, FF,GG,HHH,LLL,HDP,HDM, LDAWAY 

INTEGER DV 


aaaanqaaaaa 


LATERAL HYDRODYNAMIC COEFFICIENTS 


aaa 


REAL YPDOT ,YRDOT,YPQ ,YQR 

REAL YVDOT ,YP 7YR +YVQ a 
REAL YWP ,YWR.,YV ,YVW 

REAL YDR ,CDY 


NORMAL HYDRODYNAMIC COEFFICIENTS 


aaa 


REAL ZQDOT ,ZPP,ZPR ,ZRR 

REAL ZWDOT ,ZQ ,ZVP ;ZVR 

REAL ZW .,ZVV ,2DS ,ZDB 

REAL ZQN ,ZWN ,ZDSN ,CDZ 
REAL ZHADOT, ZHAT 


ROLL HYDRODYNAMIC COEFFICIENTS 


aan 


REAL KPDOT ,KRDOT ,KPQ ,KQR 
REAL KVDOT , KP ,KR KVQ 
REAL KWP , KWR KV ,KVW 
REAL KPN » KDB 


PITCH HYDRODYNAMIC COEFFICIENTS 


aaa 


REAL MQDOT ,MPP ,MPR,MRR 
REAL MWDOT , MQ ,MVP ,»MVR 
REAL MW, MVV »MDS ,MDB 
REAL MQN , MWN ,MDSN 
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REAL QHADOT,QHAT,THADOT, THAT 


Cc 
Cc YAW HYDRODYNAMIC COEFFICIENTS 
Cc 
REAL NPDOT,NRDOT,NPQ ,NOR 
’ REAL NVDOT , NP ,NR »NVQ 
REAL NWP , NWR ,NV _ ,NVW 
REAL NDR 
Cc 
: Cc MASS CHARACTERISTICS OF THE FLOODED VEHICLE 
Cc 
REAL WEIGHT , BOY ,VOL ,XG 
REAL YG , 2G ,XB  ,ZB 
REAL IX , IY ,12 fIRe 
REAL 1¥Z , IXY’ ,YB 
REAL L , RHO ,G ,NU 
REAL AO’ ,KPROP ,NPROP , X1ITEST 
REAL DEGRUD  ,DEGSTN 
COMMON /BLOCK1/ F(12), FP(6), XMMINV(6,6), UCF(4) 
INTEGER N,IA,IDGT,1ER,LAST,J,K,M,JJ,KK,1 
REAL WKAREA(54), X(12) 
Cc : 
Cc RUDDER COEFFICIENTS 
Cc 
PARAMETER ( DSMAX= -0.175) 
Cc 
Cc LONGITUDINAL HYDRODYNAMIC COEFFICIENTS 
Cc 
PARAMETER(XPP = 7.E-3 ,XQQ = -1.5E-2 ,XRR = 4.E-3 ,XPR =7.5E-4, 
& XUDOT=-7.6E-3 ,XWQ = -2.E-1 ,XVP = -3.E-3  ,XVR = 2.E-2, 
& XQDS=2.5E-2 »XQDB=-2.6E-3 ,XRDR= -1.E-3 ,XVV =5.3E-2, 
2 & XWW =1.7E-1 ,XVDR=1.7E-3 ,XWDS=4.6E-2 ,XWDB= 1.E-2, 
& XDSDS= -1.E-2 ,XDBDB= -8.E-3 ,XDRDR= -1.E-2 ,XQDSN= 2.£-3, 
& XWDSN=3.5E-3 ,XDSDSN= -1.6E-3 ) 
é 
Bg Cc LATERAL HYDRODYNAMIC COEFFICIENTS 
| Cc 
PARAMETER( YPDOT#=1.2E-4 ,YRDOT=1.2E-3 ,YPQ = 4.E-3 ,YQR =-6.5E-3, 
& YVDOT=-5.5E-2 ,YP = 3.E-3 ,YR = 3,E-2 ,YVQ =2.4E-2, 
& YWP =2.3E-1 ,;YWR =-1.9E-2 ,YV = -1.E-1 ,YVW =6.8E-2, 
& YDR =2.7E-2 ,CDY =3.5E-1) 
Cc 
Cc NORMAL HYDRODYNAMIC COEFFICIENTS 
c 
PARAMETER (ZQDOT=-6.8E-3 ,ZPP =1.3E-4 ,2PR =6.7E-3 ,ZRR =-7.4E-3, 
& ZWDOT=-2.4E-1 ,2Q =-1.4E-1 ,»ZVP =-4,.8E-2 ,ZVR =4.5E-2, 
& ZW = -3.E-1 ,ZVV =-6.8E-2 ,2DS =-7.3E-2 ,ZDB =-2.6E-2, 
& ZON =-2.9E-3 ,ZWN =-5.1E-3 ,ZDSN= ~1.E-2  ,CDZ = 1.0) 
Cc 
Cc ROLL HYDRODYNAMIC COEFFICIENTS 
Cc : ; 
PARAMETER( KPDOT= -1.£-3 ,;KRDOT=-3.4E-5 ,KPQ =-6.9E-5 ,KQR =1.7E-2, 
& KVDOT=#1.3E-4 , KP =-1.1E-2 »KR =-8.4E-4 »KVQ=-5.1E-3, 
& KWP =-1.3E-4 » KWR #1.4E-2 7KV =3.1E-3 7KVW =-1.9E-1, 
& KPN =-5.7E-4 , KDB = 0.0) 
Cc 
? Cc PITCH HYDRODYNAMIC COEFFICIENTS 
Cc 


PARAMETER(MQDOT=-1.7E-2 ,MPP =5.3E-5 ,MPR = 5.E-3 ,MRR =-2.9E-3, 
& MWDOT=-6.8E-3 , MQ =-6.8E-2 MVP =1.2E-3 »MVR =1.7E-2, 
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& MW = 1.E-1 , MVV =-2.6E-2 »MDS =-4.1E-2 »MDB =6.9E-3, 


& MON =-1.6E-3 » MWN =-2.,9E-3 »MDSN =-5.2E-3) 
Cc 
Cc YAW HYDRODYNAMIC COEFFICIENTS 
Cc 
PARAMETER (NPDOT=-3.4E-5 ,NRDOT=-3.4E-3,NPQ =-2.1E-2 ,NQR =2.7E-3, 
& NVDOT#=#1.2E-3 » NP =-8.4E-4 »NR =-1.6E-2 *NVQ = -1.E-2, 
& NWP =-1.7E-2 + NWR =7.4E-3 NV =-7.,4E-3 +NVW =-2,.7E-2, . 
& NDR =-1.3E-2) 
c 
Cc MASS CHARACTERISTICS OF THE FLOODED VEHICLE 
Cc 
PARAMETER( WEIGHT =12000., BOY =12000. ,VOL =200. ,xG = 0. ’ 
& YG = 0.0 , 2G = 0.20 XB = 0. ,2B = 0.0, .. 
& IX = 1500. , TY = 10000. ,IZ = 10000. ,IXZ = -10. , 
& I¥Z = -10. , IXY = -10. ,Y¥B = 0.0, 
& L = 17.4 », RHO = 1.94 1G = 32.2 »NU = 8.47E-4 , 
& AO = 2.0 © +KPROP = 0. »NPROP = 0. , XITEST= 0.1 , 
& DEGRUD= 0.0 +DEGSTN=. 0.0) 
c 
Cc INPUT INITIAL CONDITIONS HERE IF REQUIRED 
Cc ’ 
OPEN(20,FILE='DAWAY.DAT’ ,STATUS='NEW’ ) 
OPEN(18,FILE='"MODEL.DAT’ ,STATUS='NEW’ ) 
NUMPTS=0.0 
DV=1.0 
Cc 
CRRA KKKEKKERKEKEKKKEKKKEKOBTAIN INITIAL INFORMATION# AR RKKKKKKKKKRKEAKK 
, at 
WRITE(*,*) "PROVIDE INITIAL CONDITIONS FOR SPEED AND RPM?‘ 
READ(*,*) U0O,RPM 
WRITE(*,*) "FIRST WAY POINT IN SHIP LENGTHS (XD,YD,2D)?° 
READ(*,*) XD,YD,COMZ 
WRITE (*,*) ‘WHAT IS THE DESIRED SPEED?’ 
READ(*,*) UD : 
WRITE(*,*) ‘HOW LONG BETWEEN UPDATES FROM THE NAVIGATOR?’ . 
READ (*,*). NAVUPDATE 
WRITE(*,*) © INPUT TARGET RADIUS IN SHIP LENGTHS.’ 
READ (*,*) TARGET ; ; is 
WRITE(*,*) "INPUT DRIFT VELOCITIES UCO,VCO,WCO IN FT/SEC.’ 
READ (*,*) UCO,VCO,WCO 
WRITE(*,*) ‘INPUT DRIFT VELOCITIES EST.UCE,VCE,WCE IN FT/SEC.’ 
READ (*,*) UCE,VCE,WCE 
XD1=0.0 
YD1=0.0 
XD2=XD 
YD2=YD 
“DY= (YD2-YD1) 
DX= (XD2-XD1) 
ALPHA=ATAN2 (DY, DX) 
Cc 
v0 = 0.0 
wo = 0.0 
PO = 0.0 
Q0 = 0.0 
RO = 0.0 
PHIO = 0.0 
THETAO = 0.0 
PSIO = 0.0 : 
XPOS0=0.0 
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¥POS0=0.0 
ZPOS0=0.0 
DB= 0.0 
DS = 0.0 
DR = 0.0 
LATYAW = 0.0 
NORPIT = 0.0 
RE = UO*L/NU 
TNAV=0 
XA=XPOSO 
YA=YPOSO0 
LDAWAY = 2.0 
NEARMISS = 0.0 


movecc 
euud 
ba] 
o 


XPOS = XPOSO 
YPOS = YPOS0 ’ 
ZPOS = ZPOS0 
PSI = PHIO 
THETA = THETAO 
PHI = PHIO 
QHADOT=0.0 
THADOT=0.0 
ZHADOT=0.0 
QHAT=0.0 
THAT=0 .0 
ZHAT=0.0 
VHAT=0.0 
“ ZOBSDOT=0.0 
ZZ20BS = 0.0 


DEFINE LENGTH FRACTIONS FOR GAUSS QUADRATURE TERMS 


a-AN 


0.069431844 
0.330009478 
0.669990521 
0.930568155 


WEIGHT FRACTIONS FOR GAUSS QUADRATURE TERMS 


aaa 
9 
iso] 
ia} 
~ 
2 
tt 


GK4(1) = 0.1739274225687 
GK4(2) = 0.3260725774312 
GK4(3) = 0.3260725774312 
GK4(4) = 0.1739274225687 


DEFINE THE BREADTH BB AND HEIGHT HH TERMS FOR THE INTEGRATION 


aaaAA 


75.7/12 
75.7/12 
75.7/12 
55.08/12 © 


(1) = 16.38/12 
HH(2) = 31.85/12 
(3) = 31.85/12 
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HH(4) = 23.76/12 
MASS = WEIGHT/G 


N = 6 
pO 15 J = 1,N 
po 10 K = 1,N 
XMMINV(J,K) = 0.0 * 
MM(J,K) = 0.0 
10 CONTINUE 
15 CONTINUE 


) = MASS -((RHO/2)*(L**3)*XUDOT) 
) = MASS*ZG 
) = -MASS*YG 


MM(1 


) = MASS +((RHO/2)*(L**3)*YVDOT) 
) = -MASS*ZG —((RHO/2)*(L**4)*YPDOT) 
) = MASS*XG - ((RHO/2)*(L**4)*YRDOT) 


) = MASS - ((RHO/2)*(L**3)*ZWDOT) 
) = MASS*YG 
) = -MASS*XG -((RHO/2)*(L**4)*ZQDOT) 


= -MASS*2G -— ((RHO/2)*(L**4)*KVDOT) 
= MASS*YG 

= IX — ((RHO/2)*(L**5) *KPDOT) 

= —IXY 

= ~IXZ -—((RHO/2)*(L**5)*KRDOT) 


ee ee ee ee ee 


= MASS*2ZG 

= -—MASS*XG -((RHO/2)*(L**4)*MWDOT) 
= —IXY 

= TY —((RHO/2)*(L**5)*MQDOT) 

= YZ 


= —MASS*YG 
= MASS*XG ~—((RHO/2)*(L**4)*NVDOT) 
= -IXZ - ((RHO/2)*(L**5)*NPDOT) 
= —IYZ 
MM(6,6) = 12 - ((RHO/2)*(L**5)*NRDOT) 
LAST=N*N+3*N 
DO 20 M=1,LAST 
WKAREA(M) = 0.0 
20 CONTINUE 


IER = 0 
IA = 6 
IDGT = 4 
Cc *****ROUTINE FOR INVERTING THE MM MATRIX**##x 
DO 12 I=1,N 
DO 11 J=#1,N 
. XMMINV(I,J)=0.0 
11 CONTINUE 
XMMINV(I,1I)=1 
12 CONTINUE 
CALL INVTA(MM,N,INDX,D) 
pO 13 J=1,N 
CALL INVTB(MM,N,INDX,XMMINV(1,J)) 4 
13 - CONTINUE 
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RIKER RK INPUTS KR KR RK RK 


RUDDER AND DIVE PLANE COMMANDS 


aAaAANA 


WRITE(*,*) ‘INPUT SIMULATION TIME AND TIME STEP’ 
READ (*,*) SIM,DELT 


TIME=0.0 
7 pS= 0.0 
DR= 0.0 
DB= 0.0 
EITA=4.0 
» BAR=.4 
C SIZE OF OUTPUT DATA ARRAY FOR PLOTTING 
NUMOUT=6 
711 FORMAT (214) 
Cc 
Cc KKK RIAKAIAKKEKEKKEKST MULATION BEGINS KEKKKKKKEKKAKKKEKE 
po 100 I=#1,SIM 
c 
c PROPULSION MODEL 
c 
SIGNU = 1.0 
IF (U.LT.0.0) SIGNU = -1.0 
IF (ABS(U).LT.X1ITEST) U = XITEST 
SIGNN = 1.0 
IF (RPM.LT.0.0) SIGNN = -1.0 
ETA = 0.012*RPM/U 
RE = U*L/NU 
cp0 = .00385 + (1.296E-17)*(RE ~- 1.2E7)**2 
CT = ABS(0.008*L**2*ETA*ABS(ETA)/(A0) ) 
CTL =ABS( 0.008*L**2/(A0)) : : 
EPS = 1. 0+SIGNN/SIGNU* (SQRT(CT+1.0)-1.0)/(SQRT(CT1+1.0)-1.0) 
XPROP = CDO*(ETA*ABS(ETA) ~- 1.0) 
c 
2 c 
c 
c CALCULATE THE DRAG FORCE, INTEGRATE THE DRAG OVER THE VEHICLE 
Cc INTEGRATE USING A 4 TERM GAUSS QUADUTURE 
‘ c 


LATYAW = 0.0 
NORPIT = 0.0 
po 500 K = 1,4 
UCF(K) = SQRT((V+G4(K)*R*L)**2 + (W-G4(K)*Q*L)**2) 
IF(UCF(K).GT.1E-10) THEN 
= TERMO = (RHO/2) *(CDY*HH(K)*(V+G4(K)*R*L)**2 + 
& CDZ*BR(K)*(W-G4(K)*Q*L) **2) 


TERML = TERMO*(V+G4(K)*R*L)/UCF(K) 
TERM2 = TERMO*(W-G4(K)*Q*L) /UCF(K) 
LATYAW = LATYAW + TERM1*GK4(K)*L 
NORPIT = NORPIT + TERM2*GK4(K)*L 
END IF 


CONTINUE 


500 

Cc . 

Cc FORCE EQUATIONS 

Cc 

Cc LONGITUDINAL FORCE 
Cc 


FP(1) = MASS*V*R - MASS*W*Q + MASS*XG*Q**2 + MASS*XG*R**2- 
. & MASS*YG*P*Q ~ MASS*ZG*P*R + (RHO/2)*L**4*(XPP*P**2 + 
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a-AA 


aaA Aaa aaa aaa 


qaaaa 


MMMM sAm mM 


DMM Am MH 


BDMMMmMs 


XQQ*Q**2 + XRR*R**2 + KPR*P*R) +(RHO/2)*L**3*(XWQAWAQ + 
XVP*V*P4+XVRAV*R+U*Q* (XODS*DS+XQDB*DB)+XRDR*U*R*DR) + 
(RHO/2) *L**2*(XVVAVER2 + XWWHW*A2 + XVDR*AUAVADR + URW 
(XWDS*DS+XWDB* DB) +U**2* (XDSDS*DS**2+XDBDB*DB* *2+ 
XDRDR*DR**2))-(WEIGHT -BOY)*SIN(THETA) +(RHO/2)*L**3% 
XQDSN*U*Q*DS*EPS+( RHO/2 ) *L**2* ( XWDSN*U*W*DS+XDSDSN*UK*2* 
DS**2)*EPS +(RHO/2)*L**2*U**2*XPROP 


LATERAL FORCE 


FP(2) = —-MASS*U*R — MASS*XG*P*Q + MASS*YG*R**2 — MASS*ZG*Q*R + 
(RHO/2)*L**4*(YPQ*P*Q + YOR*Q*R)+(RHO/2)*L**3*(YP*UAP + “ 
YR*U*R + YVQ*V*Q + YWP*W*P + YWR*W*R) + (RHO/2)*L**2* 
(YV*U*V +. YVW*V*W +YDR*U**2*DR) -LATYAW +(WEIGHT-BOY )* 
COS ( THETA) *SIN( PHI )+MASS*W* P+MASS* YG* Px *2 


MDM BW W 


NORMAL FORCE 


FP(3) = MASS*U*Q - MASS*V*P — MASS*XG*P*R - MASS*YG*Q*R + 
MASS*ZG*P**2 + MASS*ZG*Q**2 + (RHO/2)*L**4*(ZPPRPAAQ + 
ZPR*P*R + ZRR*R**2) + (RHO/2) *L**3*(ZQ*URQ + ZVPAVEP + 
ZVR*EV*R) +(RHO/2) *L**2*(ZWHURW + ZVVEVAHZ + U*K2*(ZDS* 
DS+ZDB*DB) )-NORPIT+(WEIGHT-BOY ) *COS( THETA) *COS( PHI )+ 
(RHO/2)*L**3*ZQN*U*Q*EPS +(RHO/2)*L**2*(ZWN*U*W +ZDSN* 
U**2*DS) * EPS 


MM MAM MAM 


ROLL FORCE 


FP(4) = -IZ*Q*R 41¥*Q*R -IXY*P*R +IYZ*Qe*2 -LYZ*R**2 +I XZ*PA*Q + 
MASS*YG*U*Q -MASS*YG*V*P -MASS*ZG*W*P+(RHO/2) *L**5* ( KPQ* 
P*Q + KQR*Q*R) +(RHO/2)*L**4%*(KP*XU*P 4+KR*UYR + KVQ*VEQ + 
KWP*WAP + KWR*W*R) +(RHO/2) *L**3*(KV*URV + KVWEVEW) + 
(YG*WEIGHT - YB*BOY)*COS(THETA)*COS(PHI) - (ZG*WEIGHT - 
ZB*BOY)*COS(THETA)*SIN(PHI) + (RHO/2)*L**4*KPN*U*P*EPS+ 
(RHO/2)*L**3*U**2*KPROP +MASS*ZG*U*R - 


PITCH FORCE 


FP(5) = -IX*P*R +1Z*P*R +IXY*Q*R -LTYZ*P*Q -IXZ*P**2 +IXZ*Re*2Z — 
MASS*XG*U*Q + MASS*XG*V*P + MASS*ZG*V*R -— MASS*ZG*W*Q + 
(RHO/2)*L**5*(MPP*P**2 +MPR*P*R +MRR*R**2)+(RHO/2) *L**4* 
(MQ*U*Q + MVP*V*P + MVR*V*R) + (RHO/2)*L**3*(MW*URW + 
MVV*V4%*24U*%*2* (MDS*DS+MDB*DB))+ NORPIT -(XG*WEIGHT- 

_XB* BOY) *COS( THETA) *COS( PHI) +(RHO/2)*L**4*MQN*U*Q*EPS | + 
(RHO/2 ) *L**3* (MWN*U*W4+MDSN*U* *2*DS ) XEPS— : 
(Z2G*WEIGHT-ZB*BOY)*SIN( THETA) 


WHNMM MOM 


YAW FORCE 


FP(6) = -IY*P*Q +IX*P*Q +1IXY*P**2 -IXY*Q**2 +IYZXP*R -IXZ*Q*R — 
MASS*XG*U*R + MASS*XG*W*P — MASS*YG*V*R + MASS*YG*W*Q + 
(RHO/2)*L**5*(NPQ*P*Q + NOR*Q*R) +(RHO/2)*L**4*(NPXUXP+ 
NR*UFR + NVQ*U*Q +NWP*W*P + NWR*W*R) +(RHO/2) *L**3* (NV* 
U*V + NVW*V*W + NDR*U**2*DR) - LATYAW + (XG*WEIGHT — 

XB* BOY) *COS( THETA) *SIN( PHI )+(YG*WEIGHT) *SIN( THETA) 
+(RHO/2 ) *L**3*U**2 *NPROP-YB*BOY*SIN( THETA) 


NOW COMPUTE THE F(1-6) FUNCTIONS “ 
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po 600 J = 1,6 
F(J) = 0.0 
po 600 K = 1,6 
F(J) = XMMINV(J,K)*FP(K) + F(J) 


600 CONTINUE 
Cc 
c THE LAST SIX EQUATIONS COME FROM THE KINEMATIC RELATIONS 
Cc 
. Cc FIRST SET THE DRIFT CURRENT VALUES 
Cc 
Cc uco = 0.0 
c vco = 0.0 
7 Cc woo = 0.0 
Cc . ; 
Cc INERTIAL POSITION RATES F(7-9) 
Cc 
F(7) = UCO + U*COS(PSI)*COS(THETA) + v*(COS(PSI)*SIN( THETA) * 
& SIN(PHI) .- SIN(PSI)*COS(PHI)) + W*(COS(PSI)*SIN( THETA) * 
& COS(PHI) + SIN(PSI)*SIN( PHT) ) 
F(8) = VCO + U*SIN(PSI)*COS(THETA) + V*(SIN(PSL)*SIN( THETA) * 
& SIN(PHI) + COS(PSI)*COS(PHI)) + wW*(SIN(PSI)*SIN( THETA) * 
& COS(PHI) - COS(PSI)}*SIN(PHI)) 
Cc 
F(9) = WCO -— U*SIN(THETA) +V*COS( THETA) *SIN( PHI) +W*COS ( THETA) * 
& COS (PHI) 
c 
Cc EULER ANGLE RATES F(10-12) 
Cc 
F(10) = P + Q*SIN(PHI)*TAN(THETA) + R*COS( PHI) *TAN( THETA) 
Cc 
F(11) = Q*COS(PHI) - R*SIN(PHI) 
Cc ‘ 
F(12) = Q*SIN(PHI)/COS(THETA) + R*COS( PHI) /COS( THETA) 
c 
= upoT = F(1) 
VDOT = F(2) 
WDOT = F(3) 
PDOT = F(4) 
‘ QDOT = F(5) 
RDOT = F(6) 
XDOT = F(7) 
YDOT = F(8) 
ZDOT = F(9) 
PHIDOT = F(10) 
THETAD = F(11) 
‘PSIDOT = F(12) 
Cc 
Cc kkeERKKK «CREATE OUTPUT DATA FILE X& RRR RRR RRR IIHR RRR KE 
Cc 


IF (I .EQ. DV ) THEN 
TIMER=FLOAT(1I)/2. 
WRITE (20,*) I 
WRITE (20,744) DAWAY 
WRITE (18,*) I 
WRITE (18,743) DS/.01745 ,DR/.01745 
WRITE (18,744) XPOS/L,YPOS/L,ZPOS/L,XD,¥D,COMZ, DAWAY 
WRITE (18,745) U,UD,RPM | 
743 FORMAT (2E11.3) 
: 744 FORMAT (6£12.4) 
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745 


YF AANANQAAAAN AAA A AAA AHA A A ANA 


aaAaaaa 


FORMAT (6E£12.4) 
NUMPTS=NUMPTS + 1 
DV=DV+1.0/DELT 
ENDIF 


FIRST ORDER INTEGRATION 


U = U + DELT*UDOT 
U = SURGE RATE 
V = V + DELT*VDOT , 
V = SWAY RATE 
Wo= W + DELT*WDOT 
W = HEAVE RATE 
P = P + DELT*PDOT ; 
P = ROLL RATE 
Q = Q + DELT*QpOT 
fe Q = PITCH RATE 
R = R + DELT*RDOT 
R = YAW RATE 
XPOS = XPOS + DELT*XDOT 
X = SURGE 
YPOS = YPOS + DELT*YDOT ; 
Y = SWAY 
ZPOS = ZPOS + DELT*ZDOT 
Z = HEAVE 
PHI = PHI + DELT*PHIDOT 
PHI = ROLL 


THETA = THETA + DELT*THETAD 
THETA = PITCH 
PSI = PSI + DELT*PSIDOT 


PSI = YAW 
PRR RIK IR III RR IORI IO RRR OR IRR SOI IIR IOI OTTO TOI TOR IIT ATK ATR RA I IK 
* ot 
‘ok : CONTROL LAWS * 
* * 


FRI KIRK AKI KIKI KKK KEK KIKI KEK K IKE KEKE KKAKEEKKKRKKAKKKKKKAKRKKK 


FORO ITO RR TO tok SLIDING MODE DEPTH CONTROL *KKKKKKRAAWKKAKEKER 
CALL OBSER(QHADOT, THADOT, ZHADOT, QHAT, THAT, ZHAT,DELT, ZPOS,DS,U0) 


S=QHAT + 0.52*THAT - 0.0112*(ZHAT-COMZ*L) 
IF(ABS(S) .LT. BAR) SAT=(S/BAR) 

IF(S .LE. -BAR) SAT=-1.0 

IF(S .GE. BAR) SAT=1.0 

UHAT=-5.1429*QHAT + 1.0714*THAT 
UBAR=EITA*SAT 

DE=UHAT+UBAR 

IF (DE .GE. 0.4) DS=0.4 

IF (DE .LE. -0.4) DS=-0.4 

IF( (DE .LT. 0.4) .AND. (DE .GT. -0.4)) DS=DE 
DB=-DS*1.0 , 


KkkkKAKK KKK KEKK SLIDING MODE STEERING CONTROL 4884 AKKANKKKK KR 


KKEKKEKAKKEKHEKREKKKKAKKKEKKKKAK PLANNER KKK KKIREKKEAKKKKEKKKAKKKKKK 


DAWAY=( (XPOS/L-XD) **24+(YPOS/L-YD) **2)**,5 

IF ( DAWAY .LE. TARGET ) THEN 
_WRITE(*,*) ‘CURRENT POSITION IS ',XPOS/L,YPOS/L,ZPOS/L 
WRITE(*,*) ’SIMULATION TIME IS ',I 
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WRITE(*,*) ‘WHAT IS THE NEXT WAY POINT (XD,YD,Z2D)?° 

READ (*,*) XD,YD,COMZ 

XD1=XD2 ; 

YD1=YD2 

XD2=XD 

YD2=#YD 
z DY= (YD2-yYD1) 

DX= (XD2-XD1) 

ALPHA=ATAN2 (DY ,DX) 

WRITE (*,*) ‘WHAT IS THE DESIRED SPEED?’ 
~ READ (*,*) UD 

ENDIF 


KHRKAKKAKKKKKAKKEAKKKKKKKIEKK NAVIGATOR KRHRKKKAKKKKKEKEKKEKEKK EK 


aaa 


IF ((TIME-TNAV) .GE. NAVUPDATE ) THEN 
XA=XPOS/L 
YA=YPOS/L 
TNAV=TNAV+NAVUPDATE 
ENDIF 
c FO RI SORIA III HEADING (2G IO OIG IOI TORII IIR 
c ‘ 
IF(ABS(XD-XA) .LE.0.00000001) THEN 
: IF((YD-YA).GT.0.0) HD = 2.*ATAN(1.) 
IF((YD-YA).LT.0.0) HD = - 2. *ATAN(1.) 
ELSEIF ((XD-XA).LT.0.0 .AND. (YD-YA) .GT. 0.0) THEN 
HD = ATAN( (YD-YA) /( XD-—XA) } + 4.*ATAN(1.) 
ELSEIF ((XD-XA).LT.0.0 -AND. (YD-YA) .LT. 0.0) THEN 
HD = ATAN( (YD-YA)/(XD-XA) ) -~ 4.*ATAN(1.) 
ELSE ; 
HD = ATAN((YD~YA)/(XD-XA) ) 
ENDIF 
HDMDEG = HDM*360./(8.*ATAN(1.)) 


khhh kk RRR AKA ARR SWAY VELOCITY OBSERVER #4 RRA RKKRRAK KR EK 
VHAT = 0.0 
THE VALUE OF THE COEFFICIENTS VARY WITH SPEED TO ACCOUNT FOR DIFFERENCES 


BETWEEN THIS NONLINEAR MODEL AND THE LINEARIZED MODEL THE OBSERVER IS 
BASED ON.(THIS CODE WAS NOT USED, INSTEAD VHAT WAS SET EQUAL TO ZERO.) 


FF = -2.0 
LLL= -2.275*(U**6)+48.102*(U**5)-406 839% (U**4 ) + 
*& 1763.181*(U**3)-4165.344*(U**2)4+5265.448*U-3211.956 
GG = 1.1329*u - 70.535 ; 
HHH = .07584*(U**2) - 3.0509*U + .0003 
ZZOBSDOT = HHH*DR + GG*R + FF*VHAT 
ZZOBS = ZZ0BS + ZZOBSDOT*DELT 
VHAT = ZZOBS + LLL*R 


aaangaanaaaaaagaaqaaq aan 


kth kaRR AAR AHH CONDITIONS TO ALLOW SHORTEST TURN ****#RRRR KKK 


HDP = HD + 8.*ATAN(1.) 
IF (ABS(HDP-PSI) .LT. ABS(HD-PSI)) THEN 
HDM = 8*ATAN(1.) + HD 
ELSE 
- . HDM=HD 
ENDIF 
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100 


kkthk hhh kkithkhkhkeke BEGIN HEADING SMC 


VCC=VCE*COS(ALPHA) -UCE*SIN(ALPHA) 
SS2=.4767*(PSI-HDM) + .O121*VHAT + 


CALCULATIONS RRR KKKKKKKKKE 


-8790*R + 
-10) 


-4767*ASIN(VCC/U) 


.5081*R- 


IF(ABS(SS2) .LT. .1) SATSGN2=(SS2/ 
IF(SS2 .LE. -~.1) SATSGN2=#-1.0 

IF(SS2 .GE. .1) SATSGN2=1.0 

DR= 3.1007*SATSGN2 + .1373*VHAT + 

IF (DR .GE. 0.4) DR = 0.4 

IF (DR .LE. ~ 0.4) DR = -0.4 

KEKKKHKKEKEKKKKKEKKKKK RPM INPUT CALCULATION KAKKKAKAKKKKEKKKKEK 
SS1=U-UD 

IF(ABS(SS1) .LT. 1.0) SATSGN1=(SS$1/1) 

IF(SS1 .LE. -1.0) SATSGN1=-1.0 

IF(SS1 .GE. 1.0) SATSGN1=1.0 


RPM=-4000.0*SATSGN1 + 83.33%*U 
IF (RPM .GE. 5000.0) RPM= 5000.0 
IF (RPM .LE. -5000.0) RPM=-5000.0 


TIME=TIME+DELT 


PHIANG = PH1I/0.0174532925 
THEANG = THETA/0.0174532925 
PSIANG = PSI/0.0174532925 


TRAC=-YPOS 
ROLL=PHIANG 
YAW=PSTANG 
DEPTH=~ZPOS 
PITCH=THEANG 
BOWANG=(DB/.01745) 
STNANG=(DS/.01745) 


CONTINUE 

WRITE(*,*) ‘NPTS = ',NUMPTS 
WRITE(*,*) 'TIMEINTERVAL = ',DELT 
WRITE(*,*) ‘NAVIGATOR UPDATE TIME = 
WRITE(*,*) "TARGET RADIUS = ', TARGET 
WRITE(*,*) ‘NONDIM SPEED = 
WRITE(*,*) 'NEARMISS = ',NEARMISS 
. STOP 

END 


' ,NAVUPDATE 


, (UCO**2 4VCOKR2) HR L5 74 


FOROS OO RIGO DEPTH CONTROL OBSERVER # RANA KR IA ARI a RIK 


* 


SUBROUTINE OBSER(QHADOT, THADOT, ZHADOT, QHAT, THAT, ZHAT, DELT, ZPOS, 


DS,U) 


QHADOT=-0.7*QHAT-0.03*THAT-0.035*DS-20.9293* (ZPOS—ZHAT) 


THADOT=QHAT-14.4092*(ZPOS-ZHAT) 
ZHADOT=-—-6* THAT+16 .45* ( ZPOS-ZHAT) 


QHAT= QHAT+DELT*QHADOT 
THAT= THAT+DELT* THADOT | 
ZHAT= ZHAT+DELT* ZHADOT 
RETURN 

END 
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APPENDIX C. 


FOR GIGI III GOI III RII TOIT III TO III TO ITT TIA TR IK 


* THIS PROGRAM PLOTS THE OUTPUT GRAPHS FOR THE SIMULATION OF THE * 


* NONLINEAR MODEL OF THE SDV-9 SIMULATION RUNS. * 
FOR IRI IOI OIRO ROI IOI TOIT ROTOR FOR IOI TOI IIIT OTR TOR TOTO TOI IIIT TAR TIT IK 


aaqaaqaqaac 


- REAL*4 1T(600),DS(600) ,DR(600) ,OHS(600) 
REAL*4 XPOS(600) -YPOS(600),2P0S(600) 
REAL*4 ROLL(600),PITCH(600),YAW(600) 
REAL*4 RPM(600), U(600) 
REAL*4 UG(600), HG(600), uD(600), XD(600),YD(600) ,COMZ(600) 
REAL*4 DEPTH(600), DEPTHD(600), DAWAY (600) 
CHARACTER*1 ANS 
WRITE(*,*) "INPUT NPTS & THE TIME INCREMENT (DELT). 
READ(*,*) NPTS,DELT 
DO I=1,NPTS 
OPEN(18,FILE='MODEL.DAT’ ,STATUS='OLD’ ) 
READ(18,*) INC 
T(I) = DELT*INC 
READ(18,*) DS(I),DR(I) : 
READ(18,*) XPOS(I), YPOS(I),ZPOS(I), XDCE) YD(I), conz(t), DAWAY (TI) 
READ(18,*) U(I), uD(T), RPM(T) 
OHS(1)=0.0 
DEPTH(I)= -—ZPOS(I) 
DEPTHD(I) = -COMZ(I) 
ENDDO 


CALL MXMN(T,NPTS, TMX, TMN) 
CALL MXMN(DS,NPTS,DSMX,DSMN) 
CALL MXMN(DR,NPTS, DRMX, DRMN) 
CALL MXMN(XPOS,NPTS,XPOSMX, XPOSMN) 
CALL MXMN(YPOS,NPTS, YPOSMX, YPOSMN) 
. CALL MXMN( DEPTH, NPTS, DEPTHMX, DEPTHMN) 
CALL MXMN(DEPTHD,NPTS , DEPTHDMX, DEPTHDNN) 
CALL MXMN(U,NPTS,UMX,UMN) 
CALL MXMN(UD,NPTS,UDMX, UDMN) 
CALL MXMN(RPM,NPTS, RPMMX, RPMMN) 
CALL MXMN(XD,NPTS, XDMX, XDMN) 
CALL MXMN(YD,NPTS,YDMX,YDMN) —, 
. CALL MXMN(DAWAY,NPTS, DAWAYMX, DAWAYMN ) 


CALL SCALE(TMN,TMX,4.1,TORIG, TSTP, TMAX) 


IF(DSMX-DSMN .EQ. 0.0) THEN 

DSORIG=-.1 

DSSTP=.1 

DSMAX=.1 

ELSE 

CALL SCALE(DSMN,DSMX,4.1,DSORIG,DSSTP,DSMAX ) 
ENDIF 


IF(DRMX-DRMN .EQ. 0.0) THEN 
r DRORIG=—.1 

DRSTP=.1 

DRMAX=.1 

ELSE 
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CALL SCALE(DRMN, DRMX, 4.1,DRORIG, DRSTP, DRMAX) 
ENDIF 


IF ( XPOSMX- -XPOSMN EQ. 0. 0) THEN 

XPOSORIG=-.1 

XPOSSTP=.1 : 

XPOSMAX=.1 — ’ 
ELSE 

CALL SCALE(XPOSMN, XPOSMX,4.1,XPOSORIG, XPOSSTP »XPOSMAX ) 

ENDIF 


XXMX=AMAX1 ( XPOSMX , XDMX) 

XXMN=AMIN1(XPOSMN, XDMN) 

IF (XXMX-XXMN -EQ. 0.0) THEN 

XXORIG=~—.1 

XXSTP«.1 : 

XXMAX=.1 

ELSE 

CALL SCALE(XXMN,XXMX,6.1,XXORIG,XXSTP,XXMAX) - 
ENDIF 


YYMX=AMAX1 ( YPOSMX, YDMX) 
YYMN=AMIN1 ( YPOSMN, YDMN) 

- IF(YYMX-YYMN .EQ. 0.0) THEN 

YYORIG=—.1 

YYSTP=.1 

YYMAX=.1 

ELSE 

CALL SCALE(YYMN, YYMX,6.1,YYORIG, YYSTP, YYMAX) 
ENDIF 


DDMX=AMAX1 ( DEPTHMX , DEPTHDMX ) 
DDMN=AMIN1 (DEPTHMN, DEPTHDMN) 
IF(DDMX-DDMN .EQ. 0.0) THEN “ 
DDORIG=-.1 
. DDSTP=,.1 
DDMAX=.1 
ELSE 
CALL. SCALE(DDMN, DDMX,6.1,DDORIG,DDSTP , DDMAX) 
ENDIF 


IF(DAWAYMX-DAWAYMN .EQ. 0.0) THEN 
DAWAYORIG=-.1 

_ DAWAYSTP=.1 
DAWAYMAX=.1 
ELSE 
CALL SCALE(DAWAYMN, DAWAYMX,6.1,DAWAYORIG, DAWAYSTP , DAWAYMAX) 
ENDIF 


HGMX=AMAX] ( PSIMX, HDMX) 

HGMN=AMIN1(PSIMN, HDMN) 

IF (HGMX-HGMN .EQ. 0.0) THEN 

HGORIG=-.1 

HGSTP=.1 

HGMAX=.1 

ELSE 

CALL SCALE(HGMN,HGMX,6.1,HGORIG,HGSTP, HGMAX) 
ENDIF 


UGMX=AMAX] (UMX, UDMX ) 
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UGMN=AMIN1 (UMN, UDMN) 

IF(UGMX-UGMN .EQ. 0.0) THEN 

UGORIG=~.1 

UGSTP=,.1 

UGMAX=.1 

ELSE 

CALL SCALE(UGMN, UGMX,6.1,UGORIG, UGSTP, UGMAX) 
ENDIF , 


IF(RPMMX-RPMMN .EQ. 0.0) THEN 

RPMORIG=-.1 

RPMSTP=.1 

RPMMAX=. 1 

ELSE . . 

CALL SCALE(RPMMN, RPMMX,4.1,RPMORIG, RPMSTP, RPMMAX) 

ENDIF 

c 

CI IOI I III PLOT OR VIEW OPTION KIKI RIKI KEKE KKAK KKK 

c 

4 WRITE(*,*) ‘DO YOU WANT TO VIEW THE PLOT OR OBTAIN A HARDCOPY?’ 
WRITE(*,*) " ° 

WRITE(*,*) ‘INPUT 1 FOR VIEW OR 2 FOR HARDCOPY’ 

READ(*,*) IPLOT VAL 

IF(IPLOT VAL .EQ. 1) CALL PGPX 

IF(IPLOT VAL .EQ. 2) CALL LNO3I 


Aka kKA KAA AKKK HORIZONTAL POSITION *8 AR RRR RR RRR ARERR 


aaa 


CALL PAGE(8.5,11.0) 
CALL NOBRDR 
CALL PHYSOR(1.75,7.75) 
CALL AREA2D(2.0,2.0) 
CALL XTICKS(1) 
CALL YTICKS(1) 
CALL XNAME('X (SHIP LENGTHS })$',100) 
CALL YNAME(‘Y (SHIP LENGTHS )$’ ,100) 
CALL GRAF (XXORIG, XXSTP, XXMAX, YYORIG, YYSTP, YYMAX) 
IMARK=0 can 
- CALL RESET(’DOT’ ) 
CALL RESET('THKCRV’ ) 
CALL GRID(1,1) 
CALL THKCRV(4) 
CALL CURVE(XPOS, YPOS,NPTS, IMARK) 
CALL MARKER(13) 
CALL SCLPIC(2) 
IMARK=-1 
CALL CURVE(XD,YD,NPTS, IMARK ) 
CALL ENDGR(0) 


kkaRRARHAAAKERE STERN PLANE INPUTS #8 A RRR RRR RRR RRR 


CALL PHYSOR(4.95,5.05) 

CALL AREA2D(2.0,2.0) 

CALL XTICKS(1) 

CALL YTICKS(1) : 

CALL XNAME('TIME (SEC)$*,100) 

CALL YNAME('STERN PL. (DEGREES)$',100) 

CALL GRAF(TORIG, TSTP, TMAX, DSORIG, DSSTP, DSMAX) 
IMARK=0 

CALL RESET(‘DOT’) 


aaaaaaaaaaaa 
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CALL RESET(‘'THKCRV’ ) 

CALL GRID(1,1) 

CALL THKCRV( 4) 

CALL CURVE(T,DS,NPTS, IMARK) 
CALL ENDGR(0) 


RREKKKER KA KAK KKK RUDDER INPUTS 2AKKKKKAKKERR RRR 


AANAAAAAA 


CALL PHYSOR(4.95,7.75) 
CALL AREA2D(2.,2.) . 

CALL XTICKS(1) 

CALL YTICKS(1) : 
CALL YINTAX ce 

CALL XNAME('TIME (SEC)$‘',100) as 

CALL YNAME('RUDDER (DEGREES)$’,100) 

CALL GRAF(TORIG, TSTP, TMAX, DRORIG, DRSTP, DRMAX) 

CALL RESET(’DOT’ ) 

CALL RESET('THKCRV’ ) 

CALL GRID(1,1) 

IMARK=0 

CALL THKCRV(4) 

CALL CURVE(T,DR,NPTS, IMARK) 

CALL ENDGR(0) 


*xek#k** DISTANCE AWAY FROM NEXT WAY POINT (DAWAY) ***%«% 


aaa 


‘CALL PHYSOR(1.75,5.05) 

CALL AREA2D(2.,2.) 

CALL XTICKS(1) 

CALL YTICKS(1) 

CALL XNAME('TIME (SEC)$*',100) 

CALL YNAME('DAWAY (SHIP LENGTHS) $’,100) 

CALL GRAF(TORIG, TSTP, TMAX, DAWAYORIG, DAWAYSTP, DAWAYMAX ) 
CALL RESET('DOT’ ) 

CALL RESET(’THKCRV’ ) 

CALL GRID(1,1) 

IMARK=0 

CALL THKCRV(4) m 
CALL CURVE(T,DAWAY,NPTS,IMARK) : 

CALL ENDGR(0) 


KkkKKAKKAKKKKKK SPEED VS DESIRED SPEED *«kXKAKK KARA ARK 


aaAN 


CALL PHYSOR(4.95,5.05) 
CALL AREA2D(2.,2.) 
CALL XTICKS(1) 

CALL YTICKS(1) 

CALL XNAME('TIME (SEC)$',100) 

CALL YNAME('U AND UD (FT/SEC) $',100) 

CALL GRAF(TORIG, TSTP, TMAX, UGORIG, UGSTP, UGMAX) 
CALL RESET('DOT’ ) 

CALL RESET('THKCRV’ ) 

CALL GRID(1,1) 

IMARK=0 

CALL THKCRV(4) 

CALL CURVE(T,U,NPTS, IMARK) 

CALL DOT 

CALL CURVE(T,UD,NPTS, IMARK) 

CALL ENDGR(0) 
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agaeFANAANANANANNAaNaanagnagaaaaa 


101 


Ra aRN RRR RRR RIK MOTOR INPUT * aR Ri Rk ik 


CALL PHYSOR(4.95,2.35) 

CALL AREA2D(2.,2.) 

CALL XTICKS(1) 

CALL YTICKS(1) 

CALL XNAME('TIME (SEC)$’,100) 
CALL YNAME('RPM $',100) 

CALL YINTAX 

CALL GRAF(TORIG, TSTP, TMAX, RPMORIG, RPMSTP, RPMMAX ) 
CALL RESET('DOT’ ) 

CALL. RESET( 'THKCRV’” ) 

CALL GRID(1,1) 

IMARK=0 , 

CALL THKCRV(2) 

CALL CURVE(T,RPM,NPTS, IMARK) 
CALL ENDPL(0) 

GOTO 100 


CONTINUE 
CALL METAFL(1) 
CALL ENDPL(0) 
CALL DONEPL 
WRITE (*,*) ° DO YOU WANT TO CONTINUE ? (Y/N)! 
READ(*,101) ANS 
IF (ANS .EQ. '¥’) GOTO 4 
STOP 
FORMAT (A) 
END 
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APPENDIX D. 
The values used to design a redicel order observer for sway 
velocity(v) varied so much with respect to changes in surge velocity(u) that the following 
attempt was made to relate them using curve fitting techniques. The following is the 


computer code which resulted. 


RRR KARR RR KAKA RK SWAY VELOCITY OBSERVER 2RAKKKKKKKKRK KHER 


VHAT = 0.0 


THE VALUE OF THE COEFFICIENTS VARY WITH SPEED TO ACCOUNT FOR DIFFERENCES 
BETWEEN THIS NONLINEAR MODEL AND THE LINEARIZED MODEL THE OBSERVER IS 
BASED ON.(THIS CODE WAS NOT USED, INSTEAD VHAT WAS SET EQUAL TO ZERO.) 


FF = ~—2.0 
LLL= -2.275*(U**6)4+48.102*(U**5)-406.839%*(U**4) 4+ 
& 1763.181*(U**3)-4165.344*(U**2)4+5265.448*U-3211.956 


GG = 1.1329*u - 70.535 

HHH = .07584*(U**2) - 3.0509*U + .0003 
ZZOBSDOT = HHH*DR + GG*R + FF*VHAT 
ZZ0BS = ZZ0BS + ZZOBSDOT*DELT 

VHAT = ZZOBS + LLL*R 


FIGURE 48. Adaptive Sway Velocity Observer Code 


The coefficients in the above equations and the quality of the curve fitting technique 
that provided them are shown in the following graphs. Notice the wide range of values 
that occur when dealing with the full nonlinear model. This again shows the extent to 

‘which the sliding mode control laws must deal with unmodeled behavior. 


Recall that ultimately sway velocity was set equal to zero and treated as a 


disturbance. 
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BATA 


Foint #18 x=.5 Ye-2425 
Foint &2: x4 V¥s-7OF.97 
Foint #3: K21.5 ¥~-464,925 
Point #43 Kx2 V¥=-344,901 
Point #5: K=3 Ve-Z2e4.877 
Point #6: xed Y¥=~-164. 866 
Point &78 Xa5 ¥=-123.859 
Point #8: x=6 Ye-104. 854 


FALYNOMIAL MODELS YeACM YS MIA CM EDEMA CMD 94... FACT TANF OD 
Coefficients’ : 
ACO>=- 3211. 95616 
AC1>=5265.4479 
AC2)>*-4165.344 
AC3)=1763. 18129 
-AC4>e-496,83942 ' 
ACS) =48. 102117 
ACE)=-2.2753449 


Source DF $s is F 
Regression 6 1220885.259 2sO14T. 543 Zear.ar2 
Residual 1 88.26! eo.gel 
Total 7 1360973.521 
ae 
LLL 
POL YHOMILAL 


Cy my 
SPEED (ft/sec) 


Figure 49. Graph of LLL vs Time 


113 


Foint 1s K=.5 =-t. $66 
Point #2: ¥=1 Y=+2,975 
Point #3: X=1.5 V=-4.465 
Foint @43 #F2 Y=-S.798 ‘ 
Point @5: Xa ¥=2-8.47 ; 
Point 6: x4 4=-10.989 
Point #2: XaS Yu~13,.3598 
Point #8: N=é ¥s~-1$5.875S 
vv 
POLYNOMEAL MODEL? VRACH DEX M+ACM- Lect LH. HAC DORN EAC OD 
Coeffictentss . 
ACO>=. 000121149 
ACI =-3, 050896472 
AC2>=. 0787941142 
i 
Source bf ss Me F 
Regression 2 180,333 90, The 
518197060.718 N4D.3n 
Residual 5 + 868 »G00 
Total ? 186.333 
\, 
\ 
Me 
~\ 
\, 
% 
ae 
Xe. 
S. ¥ 
\ 
AS 
HHH *. c 
ss, 
ae 
Se 
N 
. oe 
% 
POLYNOMIAL NS 
ht hn en fee bh te bee 
) - ws i) in oy us + uw its) rz) t 
“ o3 (ee) r f 
SPEED (ft/sec) 
> 
Figure 50. Graph of HHH vs Speed 
. 
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BATA 


Point #13 Rae Ve-69.97 
Foint #23 id V=~69.403 
Point &35 x21.8 V=2-68.936 
Fotnt #43 nee V=-68.269 
Point @5: K=3 VY¥=8-67.136 
Point #é: Ke V=-66. 002 
Foint &75 KES Y¥=-64,869 
Foint @3: Ree y=~63,735 


POLYNOMIAL MODEL: YSACH eH MFACM- D> RRO CMO DOL. FALE ER EA BD 
Coefficients: 

ACOd#-70, S36681r es 

ACL) #1.133726649 

AC2)=-. 000024566 


Source Df $s . Ne F 
Regression 4 35.176 WV .538 
16€0213160.189 M4D. 3D 

Residual 5 - 688 PaaS 

Total ? 35.176 


GG 


ice +-6 9-97-44 re i et 
) “ a 2 


t w oy w + w w 


aa] OL 7 wv ra 
SPRED (ft/sec) 


Figure 51. Graph of GG vs Speed 
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